// get the rotation matrix that brings the coordinate frame to the camera coordinate frame
Quaternion Viewer3dParam::GetRotation()
{
// compute the rotation that brings (unit,lat) onto (Z,X)
Quaternion f = ComputeRotation( unit, lat, Vec3f(0,0,1), Vec3f(1,0,0) );
//Vec3f a = f.Rotate( unit );
//Vec3f b = f.Rotate( lat );
//Vec3f c = f.Rotate( up );
//log_dbg(DBG_INFO, "unit -> %f %f %f lat -> %f %f %f up-> %f %f %f",
//a[0], a[1], a[2], b[0], b[1], b[2], c[0], c[1], c[2]);
return f;//.Conjugate();
}
//
// LATER: remove printfs...
//
// main tracking function. Processes a given sequence of type YARPImageSequence.
int
YARPFlowTracker::Apply (YARPOutputPortOf<YARPGenericImage>& port)
{
printf ("ox %d oy %d\n", ox, oy);
// PROCESSING SEQUENCE.
int start = seq->GetStartPushing();
int stop = seq->GetStopPushing();
printf ("Processing from frame %d to %d\n", start, stop);
printf ("Sequence has %d frames\n", seq->GetNumElements());
assert (stop-start+1 <= m_maxsize);
// PREPARE SEQUENCE FOR PROCESSING.
processor.Reset ();
oseq.Reset ();
contact = 0;
contact_frame = 0;
howmanycontacts = 0;
lack_of_vectors = 0;
int i, frame;
for (frame = start; frame <= stop; frame++)
{
oseq.IncrementCounter ();
contact = processor.Apply (seq->GetImageRef (frame),
oseq.GetImageRef (frame-start));
oseq.GetImageRef (frame-start) = seq->GetImageRef (frame);
if (contact)
{
processor.GetPokeDirection (dirx, diry);
processor.GetSegmentedImage (segmentation_mask);
segmentation_mask_copy = segmentation_mask;
processor.GetFlipper(flipper_segmentation_mask);
flipper_segmentation_mask_copy = flipper_segmentation_mask;
ba.Apply(segmentation_mask_copy);
orientation = ba.GetAngle();
orientation_quality = ba.GetPointiness();
// enlarge it twice.
GrowMask (segmentation_mask, extended_segmentation);
segmentation_mask = extended_segmentation;
GrowMask (segmentation_mask, extended_segmentation);
CenterOfMass (extended_segmentation, com_x, com_y);
contact_frame = frame;
howmanycontacts++;
GenerateAndSend (port);
}
}
if (howmanycontacts == 0)
{
printf ("No poking detected... \n");
// nothing much to do.
return -1;
}
// OPTICFLOW.
int startopticflow = contact_frame - 5; //10;
if (startopticflow < start) startopticflow = start;
int endopticflow = contact_frame + FRAMES_TRACKED;
if (endopticflow > stop) endopticflow = stop;
// TRACK.
int endtracking = contact_frame+FRAMES_TRACKED;
if (endtracking > stop) endtracking = stop;
// optic flow mask, initialize optic flow and additional tracker.
mask.Zero();
YARPColorConverter::RGB2Grayscale (seq->GetImageRef(startopticflow), mono);
of.Initialize (mono);
bool trackvalid = false;
int sx = com_x, sy = com_y;
int sx2 = com_x, sy2 = com_y;
tracker.Initialize (seq->GetImageRef(startopticflow), com_x, com_y);
YarpPixelBGR green;
green.r = green.b = 0;
green.g = 0;
i = 0;
for (frame = startopticflow+1; frame <= endopticflow; frame++)
{
AddCircleOutline (oseq.GetImageRef(frame-start+1), green, sx, sy, 10);
if (tracker.IsTracking())
{
tracker.Apply (seq->GetImageRef(frame), true, sx2, sy2, trackvalid);
printf ("frame: %d, valid: %d, sx, sy: %d %d\n", frame, trackvalid, sx2, sy2);
}
YARPColorConverter::RGB2Grayscale (seq->GetImageRef(frame), mono);
if (frame < contact_frame)
of.Apply (mono, mask, sx2-sx, sy2-sy, outimage, vx[frame-start], vy[frame-start]);
else
of.Apply (mono, extended_segmentation, sx2-sx, sy2-sy, outimage, vx[frame-start], vy[frame-start]);
sx = sx2;
sy = sy2;
of.DrawFlow (oseq.GetImageRef (frame-start));
if (frame == contact_frame)
WriteMask (extended_segmentation, oseq.GetImageRef (frame-start));
if (frame >= contact_frame+1 && frame <= endtracking)
{
if (ComputeRotation (extended_segmentation,
vx[frame-start],
vy[frame-start],
ox,
oy,
trsf[i],
10) == -2)
lack_of_vectors++;
i++;
WriteMask (extended_segmentation, oseq.GetImageRef (frame-start));
}
}
CenterOfMass (extended_segmentation, final_x, final_y);
printf ("starting point: %d %d\n", com_x, com_y);
printf ("center of mass: %d %d\n", final_x, final_y);
dispframe = contact_frame - start - 10; // it was -5.
m_stepping = 2;
if (lack_of_vectors > 6)
{
printf ("optic flow is poor, skipping frames\n");
oseq.Reset ();
// adjust start frame parity.
int newstart = start;
if (((contact_frame % 2) == 0 && (newstart % 2) == 1) ||
((contact_frame % 2) == 1 && (newstart % 2) == 0)
)
newstart ++;
lack_of_vectors = 0;
printf ("re-processing from frame %d to %d\n", start, stop);
// RECOMPUTING INDEX ETC.
//
segmentation_mask = segmentation_mask_copy;
flipper_segmentation_mask = flipper_segmentation_mask_copy;
// enlarge it twice.
GrowMask (segmentation_mask, extended_segmentation);
segmentation_mask = extended_segmentation;
GrowMask (segmentation_mask, extended_segmentation);
// contact frame is ok.
// poke dir is ok.
// center of mass is ok.
// RECOMPUTE OPTIC FLOW.
int startopticflow = contact_frame - 10; //20;
if (startopticflow < newstart) startopticflow = newstart;
if (((contact_frame % 2) == 0 && (startopticflow % 2) == 1) ||
((contact_frame % 2) == 1 && (startopticflow % 2) == 0)
)
startopticflow ++;
int endopticflow = contact_frame + FRAMES_TRACKED*2;
if (endopticflow > stop) endopticflow = stop;
// TRACK.
int endtracking = contact_frame+FRAMES_TRACKED*2;
if (endtracking > stop) endtracking = stop;
YARPColorConverter::RGB2Grayscale (seq->GetImageRef(startopticflow), mono);
of.Initialize (mono);
bool trackvalid = false;
int sx = com_x, sy = com_y;
int sx2 = com_x, sy2 = com_y;
tracker.Initialize (seq->GetImageRef(startopticflow), com_x, com_y);
YarpPixelBGR green;
green.r = green.b = 0;
green.g = 0;
for (frame = start; frame <= stop; frame++)
{
oseq.IncrementCounter ();
oseq.GetImageRef (frame-start) = seq->GetImageRef (frame);
}
i = 0;
for (frame = startopticflow+2; frame <= endopticflow; frame+=2)
{
AddCircleOutline (oseq.GetImageRef(frame-start+2), green, sx, sy, 10);
if (tracker.IsTracking())
{
tracker.Apply (seq->GetImageRef(frame), true, sx2, sy2, trackvalid);
printf ("frame: %d, valid: %d, sx, sy: %d %d\n", frame, trackvalid, sx2, sy2);
}
YARPColorConverter::RGB2Grayscale (seq->GetImageRef(frame), mono);
if (frame < contact_frame)
of.Apply (mono, mask, sx2-sx, sy2-sy, outimage, vx[frame-start], vy[frame-start]);
else
of.Apply (mono, extended_segmentation, sx2-sx, sy2-sy, outimage, vx[frame-start], vy[frame-start]);
sx = sx2;
sy = sy2;
of.DrawFlow (oseq.GetImageRef (frame-start));
if (frame == contact_frame)
WriteMask (extended_segmentation, oseq.GetImageRef (frame-start));
if (frame >= contact_frame+2 && frame <= endtracking)
{
if (ComputeRotation (extended_segmentation,
vx[frame-start],
vy[frame-start],
ox,
oy,
trsf[i],
10) == -2)
lack_of_vectors++;
i++;
WriteMask (extended_segmentation, oseq.GetImageRef (frame-start));
}
}
CenterOfMass (extended_segmentation, final_x, final_y);
printf ("starting point: %d %d\n", com_x, com_y);
printf ("center of mass: %d %d\n", final_x, final_y);
printf ("improved? %d\n", lack_of_vectors);
//
//
//
m_stepping = 4;
dispframe = contact_frame - start - 10;
if (dispframe < 0) dispframe = 0;
if (lack_of_vectors > 6)
{
// bad sequence.
printf ("Still bad flow after post-processing\n");
return -2;
}
}
return 0;
}
