public StereoFrameSequenceElement(StereoFrameSequenceElement original)
{
this.RawFrame = original.RawFrame == null ? null : new Bitmap(original.RawFrame);
this.LeftRawFrame = original.LeftRawFrame == null ? null : new Bitmap(original.LeftRawFrame) ;
this.RightRawFrame = original.RightRawFrame == null ? null : new Bitmap(original.RightRawFrame);
this.TimeStamp = original.TimeStamp;
this.IsLeftFrameEmpty = original.IsLeftFrameEmpty;
this.IsRightFrameEmpty = original.IsRightFrameEmpty;
}
void StereoMEMSDataProviderCVCapFromFile__nextVideoFrameEvent(object sender, EventArgs e)
{
var rawFrame = this._videoSourceCap.QueryFrame();
if (rawFrame == null)
{
this._isVideoStreamExpired = true;
this.CheckIfAllStreamsStopped();
return;
}
var stereoFrame = this.ElementFromRawFrame(rawFrame);
rawFrame = null;
if (this._isFirstVideoFrame)
{
this._startVideoTimeStamp = stereoFrame.TimeStamp;
this._isFirstVideoFrame = false;
lock (this._videoFrameLock)
{
this._currentStereoFrame = stereoFrame;
}
}
else
{
var time = this._currentStereoFrame.TimeStamp;
double diffDT;
while ((diffDT = (DateTime.UtcNow.Subtract(time).TotalMilliseconds)) < this._framesInterval)
{
Thread.Sleep((int)(this._framesInterval - diffDT));
}
lock (this._videoFrameLock)
{
stereoFrame.TimeStamp = DateTime.UtcNow;
this._currentStereoFrame = stereoFrame;
}
}
if (this.NewStereoFrameEvent != null)
{
ThreadPool.QueueUserWorkItem(s => this.NewStereoFrameEvent(this, new NewStereoFrameEventArgs()
{
NewStereoFrame = this._currentStereoFrame
}));
}
//
while (this._isPaused && this._isStarted)
{
}
if (this._isStarted)
{
ThreadPool.QueueUserWorkItem(s => this._nextVideoFrameEvent(this, null));
}
}
//
protected void Init()
{
this._isStarted = false;
this._isPaused = false;
//this._is_videoSourceCap_ImageGrabbed_inUse = false;
//this._is_StereoMEMSDataProviderCVCapFromFile__nextAccValEvent_inUse = false;
//this._is_StereoMEMSDataProviderCVCapFromFile__nextMagnetValEvent_inUse = false;
//this._is_StereoMEMSDataProviderCVCapFromFile__nextGyroValEvent_inUse = false;
//this._is_StereoMEMSDataProviderCVCapFromFile__nextVideoFrameEvent_inUse = false;
this._isNewAccVal = false;
this._isNewMagnetVal = false;
this._isNewGyroVal = false;
_isVideoStreamExpired = false;
_isAccStreamExpired = false;
_isMagnetStreamExpired = false;
_isGyroStreamExpired = false;
this._currentMEMSSet = new MEMSReadingsSet3f();
if (this._useMEMS)
{
this._currentMEMSSet.AccVector3f = this.GetNextAccVector3f();
this._currentMEMSSet.MagnetVector3f = this.GetNextMagnetVector3f();
this._currentMEMSSet.GyroVector3f = this.GetNextGyroVector3f();
this._startMEMSTimeStampIOrig = this._currentMEMSSet.AccVector3f.TimeStampI;
this._nextAccValEvent += StereoMEMSDataProviderCVCapFromFile__nextAccValEvent;
this._nextMagnetValEvent += StereoMEMSDataProviderCVCapFromFile__nextMagnetValEvent;
this._nextGyroValEvent += StereoMEMSDataProviderCVCapFromFile__nextGyroValEvent;
}
if (this._useVideo)
{
var rawFrame = this._videoSourceCap.QueryFrame();
this._currentStereoFrame = this.ElementFromRawFrame(rawFrame);
rawFrame = null;
this._nextVideoFrameEvent += StereoMEMSDataProviderCVCapFromFile__nextVideoFrameEvent;
this._isFirstVideoFrame = true;
}
}
protected StereoFrameSequenceElement ElementFromRawFrame(Image<Bgr, byte> rawFrame)
{
rawFrame.ROI = new Rectangle(0, 0, rawFrame.Width / 2, rawFrame.Height);
var left = rawFrame.Copy();
rawFrame.ROI = Rectangle.Empty;
rawFrame.ROI = new Rectangle(rawFrame.Width / 2, 0, rawFrame.Width / 2, rawFrame.Height);
var right = rawFrame.Copy();
rawFrame.ROI = Rectangle.Empty;
StereoFrameSequenceElement res = new StereoFrameSequenceElement()
{
RawFrame = Utils.CvHelper.ConvertImageToBitmap(rawFrame),
LeftRawFrame = Utils.CvHelper.ConvertImageToBitmap(left),
RightRawFrame = Utils.CvHelper.ConvertImageToBitmap(right),
TimeStamp = DateTime.UtcNow
};
rawFrame.Dispose();
left.Dispose();
right.Dispose();
return res;
}
//interface
public override StereoFrameSequenceElement GetCurrentFrame()
{
//TODO: ditry hack
var res = new StereoFrameSequenceElement();
bool isFrameOK = false;
while (!isFrameOK)
{
try
{
lock (_currentFrameLock)
{
res.LeftRawFrame = new Bitmap(this.CurrentFrame.LeftRawFrame);
res.RightRawFrame = new Bitmap(this.CurrentFrame.RightRawFrame);
res.TimeStamp = this.CurrentFrame.TimeStamp;
isFrameOK = true;
}
}
catch { }
}
return res;
}
public override StereoFrameSequenceElement GetNextFrame()
{
lock (_currentFrameLock)
{
//dirty hack because emgu cv is f*****g shit, avoid
Image<Bgr, byte> left = null;
Image<Bgr, byte> right = null;
bool isFrameOK = false;
while (!isFrameOK)
{
try
{
left = this.LeftCapture.QueryFrame();
right = this.RightCapture.QueryFrame();
this.CurrentFrame = new StereoFrameSequenceElement()
{
LeftRawFrame = new Bitmap(left.ToBitmap()),//.Clone(),
RightRawFrame = new Bitmap(right.ToBitmap()),
TimeStamp = DateTime.UtcNow
};
isFrameOK = true;
}
catch { }
}
left.Dispose();
right.Dispose();
return this.CurrentFrame;
}
}
//interface
protected void Init()
{
this._isLeftNew = false;
this._isRightNew = false;
this.CurrentFrame = new StereoFrameSequenceElement()
{
IsLeftFrameEmpty = true,
IsRightFrameEmpty = true
};
}
public Bitmap GetFeaturesToTrack(StereoFrameSequenceElement stereoFrame, bool useGpu)
{
if (useGpu)
{
return this.GetFraturesToTrackGPU(stereoFrame).ToBitmap();
}
else
{
return this.GetFeaturesToTrackCPU(stereoFrame);
}
}
private Bitmap GetFeaturesToTrackCPU(StereoFrameSequenceElement stereoFrame)
{
throw new NotImplementedException();
}
protected Image<Bgr, byte> GetFraturesToTrackGPU(StereoFrameSequenceElement stereoFrame)
{
var modelImage = new Image<Gray, byte>(stereoFrame.LeftRawFrame);
var observedImage = new Image<Gray, byte>(stereoFrame.LeftRawFrame);
HomographyMatrix homography = null;
SURFDetector surfCPU = new SURFDetector(500, false);
VectorOfKeyPoint modelKeyPoints;
VectorOfKeyPoint observedKeyPoints;
Matrix<int> indices;
Matrix<byte> mask;
int k = 2;
double uniquenessThreshold = 0.8;
GpuSURFDetector surfGPU = new GpuSURFDetector(surfCPU.SURFParams, 0.01f);
using (GpuImage<Gray, Byte> gpuModelImage = new GpuImage<Gray, byte>(modelImage))
//extract features from the object image
using (GpuMat<float> gpuModelKeyPoints = surfGPU.DetectKeyPointsRaw(gpuModelImage, null))
using (GpuMat<float> gpuModelDescriptors = surfGPU.ComputeDescriptorsRaw(gpuModelImage, null, gpuModelKeyPoints))
using (GpuBruteForceMatcher<float> matcher = new GpuBruteForceMatcher<float>(DistanceType.L2))
{
modelKeyPoints = new VectorOfKeyPoint();
surfGPU.DownloadKeypoints(gpuModelKeyPoints, modelKeyPoints);
// extract features from the observed image
using (GpuImage<Gray, Byte> gpuObservedImage = new GpuImage<Gray, byte>(observedImage))
using (GpuMat<float> gpuObservedKeyPoints = surfGPU.DetectKeyPointsRaw(gpuObservedImage, null))
using (GpuMat<float> gpuObservedDescriptors = surfGPU.ComputeDescriptorsRaw(gpuObservedImage, null, gpuObservedKeyPoints))
using (GpuMat<int> gpuMatchIndices = new GpuMat<int>(gpuObservedDescriptors.Size.Height, k, 1, true))
using (GpuMat<float> gpuMatchDist = new GpuMat<float>(gpuObservedDescriptors.Size.Height, k, 1, true))
using (GpuMat<Byte> gpuMask = new GpuMat<byte>(gpuMatchIndices.Size.Height, 1, 1))
using (Stream stream = new Stream())
{
matcher.KnnMatchSingle(gpuObservedDescriptors, gpuModelDescriptors, gpuMatchIndices, gpuMatchDist, k, null, stream);
indices = new Matrix<int>(gpuMatchIndices.Size);
mask = new Matrix<byte>(gpuMask.Size);
//gpu implementation of voteForUniquess
using (GpuMat<float> col0 = gpuMatchDist.Col(0))
using (GpuMat<float> col1 = gpuMatchDist.Col(1))
{
GpuInvoke.Multiply(col1, new MCvScalar(uniquenessThreshold), col1, stream);
GpuInvoke.Compare(col0, col1, gpuMask, CMP_TYPE.CV_CMP_LE, stream);
}
observedKeyPoints = new VectorOfKeyPoint();
surfGPU.DownloadKeypoints(gpuObservedKeyPoints, observedKeyPoints);
//wait for the stream to complete its tasks
//We can perform some other CPU intesive stuffs here while we are waiting for the stream to complete.
stream.WaitForCompletion();
gpuMask.Download(mask);
gpuMatchIndices.Download(indices);
if (GpuInvoke.CountNonZero(gpuMask) >= 4)
{
int nonZeroCount = Features2DToolbox.VoteForSizeAndOrientation(modelKeyPoints, observedKeyPoints, indices, mask, 1.5, 20);
if (nonZeroCount >= 4)
homography = Features2DToolbox.GetHomographyMatrixFromMatchedFeatures(modelKeyPoints, observedKeyPoints, indices, mask, 2);
}
}
}
//Draw the matched keypoints
Image<Bgr, Byte> result = Features2DToolbox.DrawMatches(modelImage, modelKeyPoints, observedImage, observedKeyPoints,
indices, new Bgr(255, 255, 255), new Bgr(255, 255, 255), mask, Features2DToolbox.KeypointDrawType.DEFAULT);
#region draw the projected region on the image
if (homography != null)
{ //draw a rectangle along the projected model
Rectangle rect = modelImage.ROI;
PointF[] pts = new PointF[] {
new PointF(rect.Left, rect.Bottom),
new PointF(rect.Right, rect.Bottom),
new PointF(rect.Right, rect.Top),
new PointF(rect.Left, rect.Top)};
homography.ProjectPoints(pts);
result.DrawPolyline(Array.ConvertAll<PointF, Point>(pts, Point.Round), true, new Bgr(Color.Red), 5);
}
#endregion
return result;
}
protected void NewFrameCallback(object obj, NewFrameEventArgs e)
{
Image temp = this.currentFrame.RawFrame;
Bitmap bitmap = e.Frame;
lock (currentFrameLock)
{
if (!this.currentFrame.IsNotFullFrame)
{
var actInt = (Int64)DateTime.UtcNow.Subtract(currentFrame.TimeStamp).TotalMilliseconds;
if (actInt < this.frameInterval)
{
Thread.Sleep((int)(this.frameInterval - actInt) / 2);
}
}
this.currentFrame = this.ElementFromRawFrame(bitmap);
//event
if (NewStereoFrameEvent != null)
{
NewStereoFrameEvent(this, new NewStereoFrameEventArgs()
{
NewStereoFrame = this.currentFrame
});
}
////
this.isCurrentFrameGiven = false;
}
}
protected void Init()
{
this.currentFrame = new StereoFrameSequenceElement()
{
IsLeftFrameEmpty = true,
IsRightFrameEmpty = true
};
this.emptyFrame = new StereoFrameSequenceElement()
{
IsLeftFrameEmpty = true,
IsRightFrameEmpty = true
};
}
protected StereoFrameSequenceElement ElementFromRawFrame(Bitmap bmp)
{
Image<Bgr, byte> rawFrame = new Image<Bgr, byte>(bmp);
rawFrame.ROI = new Rectangle(0, 0, rawFrame.Width / 2, rawFrame.Height);
var left = rawFrame.Copy();
rawFrame.ROI = Rectangle.Empty;
rawFrame.ROI = new Rectangle(rawFrame.Width / 2, 0, rawFrame.Width / 2, rawFrame.Height);
var right = rawFrame.Copy();
rawFrame.ROI = Rectangle.Empty;
StereoFrameSequenceElement res = new StereoFrameSequenceElement()
{
RawFrame = rawFrame.ToBitmap(),
LeftRawFrame = left.ToBitmap(),
RightRawFrame = right.ToBitmap(),
TimeStamp = DateTime.UtcNow
};
return res;
}
