public static void Read(out CvMat matrix, XmlElement element)
{
int cols = int.Parse(element.GetAttribute("cols"));
int rows = int.Parse(element.GetAttribute("rows"));
MatrixType matType = (MatrixType)Enum.Parse(typeof(MatrixType), element.GetAttribute("type"));
string terms = element.GetAttribute("values");
List<double> values = new List<double>();
string[] words = terms.Split(',');
foreach (string w in words)
values.Add(double.Parse(w.Trim()));
switch (matType)
{
case MatrixType.F32C1:
break;
case MatrixType.F64C1:
break;
default:
throw new Exception("Read unsupported MatrixType " + matType.ToString());
}
matrix = new CvMat(rows, cols, matType);
// Fill the matrix popping values off
for (int x = 0; x < cols; ++x)
{
for (int y = 0; y < rows; ++y)
{
matrix.Set2D(x, y, new CvScalar(values[0]));
values.RemoveAt(0);
}
}
}
/// <summary>
/// CvMatを指定されたデータで埋めます
/// </summary>
/// <param name="mat"></param>
/// <param name="arr"></param>
public static void FillCvMat(CvMat mat, IList <CvScalar> arr)
{
int count = 0;
for (int i = 0; i < mat.Rows; i++)
{
for (int j = 0; j < mat.Cols; j++)
{
mat.Set2D(i, j, arr[count++]);
}
}
}
public CvMat CreateCvMat()
{
CvMat ret = new CvMat(this.Rows, this.Cols, this.ElemType);
int index = 0;
for (int y = 0; y < this.Rows; y++)
{
for (int x = 0; x < this.Cols; x++)
{
ret.Set2D(y, x, this.Array[index++]);
}
}
return ret;
}
/// <summary>
/// kalman 初期化ルーチン
/// </summary>
/// <param name="elem">読み出した要素</param>
public void kalman_update()
{
if (kalman_id == 0)
{
kalman_init();
// 初期値設定
double errcov = 1.0; //仮
kv_kalman.StatePost.Set1D(0, (float)az2_c);
kv_kalman.StatePost.Set1D(1, (float)alt2_c);
Cv.SetIdentity(kv_kalman.ErrorCovPost, Cv.RealScalar(errcov));
}
kalman_id++;
// 観測値(kalman)
//float[] m = { (float)(az2_c), (float)(alt2_c) };
//measurement = Cv.Mat(2, 1, MatrixType.F32C1, m);
measurement.Set2D(0, 0, (float)az2_c);
measurement.Set2D(1, 0, (float)alt2_c);
// 観測誤差評価 FWHM=2.35σ
double fwhm_az = 0.005 * vaz2_kv / 2.0;
double fwhm_alt = 0.005 * valt2_kv / 2.0;
double sigma_az = (fwhm_az / 2.35);
double sigma_alt = (fwhm_alt / 2.35);
kv_kalman.MeasurementNoiseCov.Set2D(0, 0, sigma_az * sigma_az);
kv_kalman.MeasurementNoiseCov.Set2D(1, 1, sigma_alt * sigma_alt);
//Cv.SetIdentity(kv_kalman.MeasurementNoiseCov, Cv.RealScalar(0.001));
// 修正フェーズ(kalman)
correction = Cv.KalmanCorrect(kv_kalman, measurement);
// 予測フェーズ(kalman)
prediction = Cv.KalmanPredict(kv_kalman);
kvaz = prediction.DataArraySingle[0]; //ans
kvalt = prediction.DataArraySingle[1]; //ans
//kvx = prediction.DataArraySingle[2];
//kvy = prediction.DataArraySingle[3];
}
public CvMat CreateCvMat()
{
CvMat ret = new CvMat(this.Rows, this.Cols, this.ElemType);
int index = 0;
for (int y = 0; y < this.Rows; y++)
{
for (int x = 0; x < this.Cols; x++)
{
ret.Set2D(y, x, this.Array[index++]);
}
}
return(ret);
}
public static void Read(out CvMat matrix, XmlElement element)
{
int cols = int.Parse(element.GetAttribute("cols"));
int rows = int.Parse(element.GetAttribute("rows"));
MatrixType matType = (MatrixType)Enum.Parse(typeof(MatrixType), element.GetAttribute("type"));
string terms = element.GetAttribute("values");
List <double> values = new List <double>();
string[] words = terms.Split(',');
foreach (string w in words)
{
values.Add(double.Parse(w.Trim()));
}
switch (matType)
{
case MatrixType.F32C1:
break;
case MatrixType.F64C1:
break;
default:
throw new Exception("Read unsupported MatrixType " + matType.ToString());
}
matrix = new CvMat(rows, cols, matType);
// Fill the matrix popping values off
for (int x = 0; x < cols; ++x)
{
for (int y = 0; y < rows; ++y)
{
matrix.Set2D(x, y, new CvScalar(values[0]));
values.RemoveAt(0);
}
}
}
// NOTE : Also seems not well written and craves optimization at places. P.A.N.A.R.G.O.
// => frame = 8 bit greyscale CvMat
static public void ContrastEnhancement(CvMat frame)
{
//CvMat originalFrame = frame; // return this if cannot enhance
//if (frame.ElemType != MatrixType.U8C1)
// frame = MatOps.Convert(frame, MatrixType.U8C1, 1 / 255.0 );
/////original histogram
const int HistBinSize = 256;
int[] histSizes = new int[1];
histSizes[0] = HistBinSize;
CvHistogram hist = new CvHistogram(histSizes, HistogramFormat.Array);
Cv.CalcArrHist(frame, hist, false); // size = 256 implied
CvHistogram newHist = MatOps.CopyHistogram(hist);
CvArr newHistBin = newHist.Bins;
//double[] origVals = new double[hist.Bins.GetDims( 0 )];
List <double> origVals = new List <double>(HistBinSize);
for (int i = 0; i < HistBinSize; i++)
{
double elem = newHistBin.GetReal1D(i);
if (elem != 0)
{
origVals.Add(elem);
}
}
// FIX : See no need for histL, since we have origVals
//////histogram with only nonzero bins
//CvMat histL = new CvMat( imageRows, imageCols, MatrixType.F32C1, new CvScalar( 0 ) );
//for (i = 0; i < origVals.size(); i++)
// histL.at<float>( i, 0 ) = origVals.at( i );
List <double> peakValues = new List <double>(HistBinSize); //std::vector<int> peakValues;
//////////3 bin search window
for (int i = 1; i < origVals.Count - 2; ++i)
{
double elem = origVals[i];
if (elem > origVals[i - 1] && elem > origVals[i + 1])
{
peakValues.Add(elem);
}
}
if (peakValues.Count == 0)
{
//Console.Out.WriteLine( "Cannot enhance" );
return; // cannot enhance?
}
//////Upper threshold
double threshUP = 0;
for (int i = 0; i < peakValues.Count; ++i)
{
threshUP += peakValues[i];
}
threshUP /= peakValues.Count;
//////Lower threshold
double threshDOWN = Math.Min((frame.Cols * frame.Rows), threshUP * origVals.Count) / 256.0;
//Console.Out.WriteLine( "Enhance thresholds " + threshUP + "/" + threshDOWN );
//////histogram reconstruction
CvArr histBins = hist.Bins;
for (int i = 0; i < HistBinSize; ++i)
{
double histElem = histBins.GetReal1D(i);
if (histElem > threshUP)
{
histBins.SetReal1D(i, threshUP);
}
else if (histElem <= threshUP && histElem >= threshDOWN)
{
continue;
}
else if (histElem < threshDOWN && histElem > 0)
{
histBins.SetReal1D(i, threshDOWN);
}
else if (histElem == 0)
{
continue;
}
}
// accumulated values(?)
double[] accVals = new double[HistBinSize]; //std::vector<int> accVals;
accVals[0] = (histBins.GetReal1D(0));
for (int i = 1; i < HistBinSize; ++i)
{
accVals[i] = (accVals[i - 1] + histBins[i]);
}
byte[] lookUpTable = new byte[HistBinSize]; //cv::Mat lookUpTable = cv::Mat::zeros( hist.size(), CV_8UC1 );
for (int i = 0; i < HistBinSize; ++i)
{
lookUpTable[i] = (byte)(255.0 * accVals[i] / accVals[255]);
}
// assign computed values to input frame
//Console.Out.Write( "Enhance-->" );
for (int i = 0; i < frame.Cols; ++i)
{
for (int j = 0; j < frame.Rows; ++j)
{
// there is NO mask, thus no need to check for; was: "if (mask.data)..."
byte oldValue = (byte)frame.Get2D(j, i);
byte newValue = lookUpTable[oldValue];
//if ((newValue <1 || newValue > 254) && (newValue != oldValue)) Console.Out.Write( oldValue + " " + newValue + "|");
frame.Set2D(j, i, newValue);
//frame.SetReal2D( j, i, lookUpTable[ (int)(255.0 * frame.GetReal2D( j, i )) ] / 255.0);
}
}
//Console.Out.WriteLine();
//frame = MatOps.Convert( frame, MatrixType.U8C1, 255.0 );
}
// 孤立輝点除去
// 周囲に輝点が無い場合，その輝点を消す
static CvMat removeNoize(CvMat image)
{
// 1px大きい作業用画像
CvMat workImage = new CvMat( image.Rows+1, image.Cols+1, MatrixType.U8C1 );
image.CopyMakeBorder( workImage, new CvPoint(1, 1), BorderType.Constant );
// 走査
for ( int row = 0; row < image.Rows; row++ )
{
for ( int col = 0; col < image.Cols; col++ )
{
// 注目画素が暗点ならば何もしない
if ( 0 == image.Get2D( row, col ))
continue;
// 範囲3x3の輝点が1ならば，中心画素を暗点にする
CvRect rect = new CvRect( col, row, 3, 3 );
CvMat area;
workImage.GetSubArr ( out area, rect );
int nonzero = area.CountNonZero();
if ( 1 == nonzero )
image.Set2D( row, col, 0 );
}
}
return image;
}
/// <summary>
/// CvMatを指定されたデータで埋めます
/// </summary>
/// <param name="mat"></param>
/// <param name="arr"></param>
public static void FillCvMat(CvMat mat, IList<CvScalar> arr)
{
int count = 0;
for (int i = 0; i < mat.Rows; i++)
{
for (int j = 0; j < mat.Cols; j++)
{
mat.Set2D(i, j, arr[count++]);
}
}
}
/// <summary>
/// CCD座標(cx,cy)->(az,alt)に変換
/// </summary>
//
// CCD座標(cx,cy):CCD中心からの誤差座標[pix] Std. Cam が基準(cx = x-xc, cy = y-yc)
// 中心位置(az_c,alt_c)と視野回転(theta_c)
// fl:焦点距離[mm], ccdpx,ccdpy:ピクセル間隔[mm]
public void cxcy2azalt(double cx, double cy,
double az_c, double alt_c, int mode, double theta_c,
double fl, double ccdpx, double ccdpy,
ref double az, ref double alt)
{
double rad = Math.PI / 180.0;
double cxmm, cymm;
//ターゲットの方向余弦
if (mode == mmEast)
{
cxmm = -cx * ccdpx; // +ccd -az
cymm = +cy * ccdpy; // +ccd +alt
}
else
{ //mmWest
cxmm = +cx * ccdpx; // +ccd +az
cymm = -cy * ccdpy; // +ccd -alt
}
CvMat v1 = new CvMat(3, 1, MatrixType.F64C1);
v1.Set2D(0, 0, fl);
v1.Set2D(1, 0, -cxmm);
v1.Set2D(2, 0, cymm);
v1.Normalize(v1);// 方向余弦化
CvMat v2 = new CvMat(3, 1, MatrixType.F64C1);
CvMat Rx = new CvMat(3, 3, MatrixType.F64C1);
CvMat Rz = new CvMat(3, 3, MatrixType.F64C1);
CvMat Ry = new CvMat(3, 3, MatrixType.F64C1);
//Rx.rotX(-theta_c * rad); // 回転マトリクスをセット
double sin = Math.Sin(-theta_c * rad);
double cos = Math.Cos(-theta_c * rad);
Rx.Set2D(0, 0, 1); Rx.Set2D(0, 1, 0); Rx.Set2D(0, 2, 0);
Rx.Set2D(1, 0, 0); Rx.Set2D(1, 1, cos); Rx.Set2D(1, 2, -sin);
Rx.Set2D(2, 0, 0); Rx.Set2D(2, 1, sin); Rx.Set2D(2, 2, cos);
//Rz.rotZ(-az_c *rad ); // 天球座標系と回転方向が逆なのでマイナス
sin = Math.Sin(-az_c * rad);
cos = Math.Cos(-az_c * rad);
Rz.Set2D(0, 0, cos); Rz.Set2D(0, 1, -sin); Rz.Set2D(0, 2, 0);
Rz.Set2D(1, 0, sin); Rz.Set2D(1, 1, cos); Rz.Set2D(1, 2, 0);
Rz.Set2D(2, 0, 0); Rz.Set2D(2, 1, 0); Rz.Set2D(2, 2, 1);
//Ry.rotY(-alt_c *rad ); // 回転マトリクスをセット
sin = Math.Sin(-alt_c * rad);
cos = Math.Cos(-alt_c * rad);
Ry.Set2D(0, 0, cos); Ry.Set2D(0, 1, 0); Ry.Set2D(0, 2, sin);
Ry.Set2D(1, 0, 0); Ry.Set2D(1, 1, 1); Ry.Set2D(1, 2, 0);
Ry.Set2D(2, 0, -sin); Ry.Set2D(2, 1, 0); Ry.Set2D(2, 2, cos);
v2 = Rz * (Ry * (Rx * v1)); // 順番注意（画像中心をx軸に一致させている状態がスタート）
// Retrun Val
az = Math.Atan2(-v2.Get2D(1, 0), v2.Get2D(0, 0)) / rad;
if (az < 0)
{
az += 360;
}
alt = Math.Asin(v2.Get2D(2, 0)) / rad;
// //Az_Cとの距離が近い値を採用
// double az2 = az - 360;
// double az3 = az + 360;
// double dis1 = Math.Abs(az - az_c);
// double dis2 = Math.Abs(az2 - az_c);
// double dis3 = Math.Abs(az3 - az_c);
// if (dis1 > dis2) az = az2;
// if (dis1 > dis3) az = az3;
}
void Setting()
{
var cameraSetting = new[]
{
1.1695652991231043e+003,
0,
6.4021949743978928e+002,
0,
1.1681614889356322e+003,
3.9445679497160154e+002,
0,
0,
1
};
var distortionSetting = new[]
{
2.6841557745990408e-001,
-1.2536292326179452e+000,
-5.1949672285252447e-003,
3.4873826852522883e-003,
1.6164774281680905e+000
};
/*
* var cameraSetting = new[]
* {
* 1.1442471246455027e+003,
* 0,
* 6.4812020158754228e+002,
* 0,
* 1.1443391554606812e+003,
* 3.8794788696276186e+002,
* 0,
* 0,
* 1
* };
*
* var distortionSetting = new[]
* {
* 2.0156935445340091e-001,
* -1.0814314304293935e+000,
* -6.7653166226057113e-003,
* 3.7355841949211124e-003,
* 1.7023014402148473e+000
* };
*/
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
_camera.Set2D(i, j, cameraSetting[i * 3 + j]);
}
}
for (int i = 0; i < 5; i++)
{
_distortion.Set2D(i, 0, distortionSetting[i]);
}
}
// Convert the Texture2D type of Unity to OpenCV's CvMat
// This uses Adcock's parallel C# code to parallelize the conversion and make it faster
// I found the code execution dropped from 180 msec per frame to 70 msec per frame with parallelization
void Texture2DToCvMat()
{
//float startTime = Time.realtimeSinceStartup;
Color[] pixels = _webcamTexture.GetPixels();
// Parallel for loop
Parallel.For(0, imHeight, i =>
{
for (var j = 0; j < imWidth; j++)
{
var pixel = pixels[j + i * imWidth];
var col = new CvScalar
{
Val0 = (double)pixel.b * 255,
Val1 = (double)pixel.g * 255,
Val2 = (double)pixel.r * 255
};
videoSourceImage.Set2D(i, j, col);
}
});
// CvScalar col;
// Color pixel;
// int i, j;
//
// // Non-parallelized code
// for (i = 0; i < imHeight; i++) {
// for (j = 0; j < imWidth; j++) {
// pixel = pixels [j + i * imWidth];
//
// col = new CvScalar
// {
// Val0 = (double)pixel.b * 255,
// Val1 = (double)pixel.g * 255,
// Val2 = (double)pixel.r * 255
// };
//
// videoSourceImage.Set2D (i, j, col);
// }
//
// }
// Flip up/down dimension and right/left dimension
if (!FlipUpDownAxis && FlipLeftRightAxis)
{
Cv.Flip(videoSourceImage, videoSourceImage, FlipMode.XY);
}
else if (!FlipUpDownAxis)
{
Cv.Flip(videoSourceImage, videoSourceImage, FlipMode.X);
}
else if (FlipLeftRightAxis)
{
Cv.Flip(videoSourceImage, videoSourceImage, FlipMode.Y);
}
// Test difference in time between parallel and non-parallel code
//Debug.Log (Time.realtimeSinceStartup - startTime);
}
// Use the CamShift algorithm to track to base histogram throughout the
// succeeding frames
void CalculateCamShift(CvMat _image)
{
CvMat _backProject = CalculateBackProjection(_image, _histogramToTrack);
// Create convolution kernel for erosion and dilation
IplConvKernel elementErode = Cv.CreateStructuringElementEx(10, 10, 5, 5, ElementShape.Rect, null);
IplConvKernel elementDilate = Cv.CreateStructuringElementEx(4, 4, 2, 2, ElementShape.Rect, null);
// Try eroding and then dilating the back projection
// Hopefully this will get rid of the noise in favor of the blob objects.
Cv.Erode(_backProject, _backProject, elementErode, 1);
Cv.Dilate(_backProject, _backProject, elementDilate, 1);
if (backprojWindowFlag)
{
Cv.ShowImage("Back Projection", _backProject);
}
// Parameters returned by Camshift algorithm
CvBox2D _outBox;
CvConnectedComp _connectComp;
// Set the criteria for the CamShift algorithm
// Maximum 10 iterations and at least 1 pixel change in centroid
CvTermCriteria term_criteria = Cv.TermCriteria(CriteriaType.Iteration | CriteriaType.Epsilon, 10, 1);
// Draw object center based on Kalman filter prediction
CvMat _kalmanPrediction = _kalman.Predict();
int predictX = Mathf.FloorToInt((float)_kalmanPrediction.GetReal2D(0, 0));
int predictY = Mathf.FloorToInt((float)_kalmanPrediction.GetReal2D(1, 0));
// Run the CamShift algorithm
if (Cv.CamShift(_backProject, _rectToTrack, term_criteria, out _connectComp, out _outBox) > 0)
{
// Use the CamShift estimate of the object center to update the Kalman model
CvMat _kalmanMeasurement = Cv.CreateMat(2, 1, MatrixType.F32C1);
// Update Kalman model with raw data from Camshift estimate
_kalmanMeasurement.Set2D(0, 0, _outBox.Center.X); // Raw X position
_kalmanMeasurement.Set2D(1, 0, _outBox.Center.Y); // Raw Y position
//_kalmanMeasurement.Set2D (2, 0, _outBox.Center.X - lastPosition.X);
//_kalmanMeasurement.Set2D (3, 0, _outBox.Center.Y - lastPosition.Y);
lastPosition.X = Mathf.FloorToInt(_outBox.Center.X);
lastPosition.Y = Mathf.FloorToInt(_outBox.Center.Y);
_kalman.Correct(_kalmanMeasurement); // Correct Kalman model with raw data
// CamShift function returns two values: _connectComp and _outBox.
// _connectComp contains is the newly estimated position and size
// of the region of interest. This is passed into the subsequent
// call to CamShift
// Update the ROI rectangle with CamShift's new estimate of the ROI
_rectToTrack = CheckROIBounds(_connectComp.Rect);
// Draw a rectangle over the tracked ROI
// This method will draw the rectangle but won't rotate it.
_image.DrawRect(_rectToTrack, CvColor.Aqua);
_image.DrawMarker(predictX, predictY, CvColor.Aqua);
// _outBox contains a rotated rectangle esimating the position, size, and orientation
// of the object we want to track (specified by the initial region of interest).
// We then take this estimation and draw a rotated bounding box.
// This method will draw the rotated rectangle
rotatedBoxToTrack = _outBox;
// Draw a rotated rectangle representing Camshift's estimate of the
// object's position, size, and orientation.
_image.DrawPolyLine(rectangleBoxPoint(_outBox.BoxPoints()), true, CvColor.Red);
}
else
{
//Debug.Log ("Object lost by Camshift tracker");
_image.DrawMarker(predictX, predictY, CvColor.Purple, MarkerStyle.CircleLine);
_rectToTrack = CheckROIBounds(new CvRect(predictX - Mathf.FloorToInt(_rectToTrack.Width / 2),
predictY - Mathf.FloorToInt(_rectToTrack.Height / 2),
_rectToTrack.Width, _rectToTrack.Height));
_image.DrawRect(_rectToTrack, CvColor.Purple);
}
if (trackWindowFlag)
{
Cv.ShowImage("Image", _image);
}
}
