/// <summary>
/// 既知の内部パラメータを用いて，それぞれのビューにおける外部パラメータを推定する．
/// 3次元のオブジェクトの点とそれに対応する２次元投影点が指定されなければならない．この関数も逆投影誤差の最小化を行う．
/// </summary>
/// <param name="objectPoints">オブジェクトの点の配列．3xNまたはNx3でNはビューにおける点の数．</param>
/// <param name="imagePoints">対応する画像上の点の配列．2xNまたはNx2でNはビューにおける点の数．</param>
/// <param name="cameraMatrix">カメラ内部行列 (A) [fx 0 cx; 0 fy cy; 0 0 1]. </param>
/// <param name="distCoeffs">歪み係数のベクトル．4x1または1x4 [k1, k2, p1, p2]．nullの場合，歪み係数はすべて0 であるとする．</param>
/// <param name="rvec">出力される 3x1 の回転ベクトル</param>
/// <param name="tvec">出力される 3x1 の並進ベクトル</param>
/// <param name="useExtrinsicGuess"></param>
#else
/// <summary>
/// Finds extrinsic camera parameters for particular view
/// </summary>
/// <param name="objectPoints">The array of object points, 3xN or Nx3, where N is the number of points in the view. </param>
/// <param name="imagePoints">The array of corresponding image points, 2xN or Nx2, where N is the number of points in the view. </param>
/// <param name="cameraMatrix">The camera matrix (A) [fx 0 cx; 0 fy cy; 0 0 1]. </param>
/// <param name="distCoeffs">The vector of distortion coefficients, 4x1 or 1x4 [k1, k2, p1, p2]. If it is NULL, all distortion coefficients are considered 0's. </param>
/// <param name="rvec">The output 3x1 or 1x3 rotation vector (compact representation of a rotation matrix, see cvRodrigues2). </param>
/// <param name="tvec">The output 3x1 or 1x3 translation vector. </param>
/// <param name="useExtrinsicGuess"></param>
#endif
public static void FindExtrinsicCameraParams2Cs(CvMat objectPoints, CvMat imagePoints, CvMat cameraMatrix, CvMat distCoeffs, CvMat rvec, CvMat tvec, bool useExtrinsicGuess)
{
if (objectPoints == null)
throw new ArgumentNullException("objectPoints");
if (imagePoints == null)
throw new ArgumentNullException("imagePoints");
if (cameraMatrix == null)
throw new ArgumentNullException("cameraMatrix");
if (rvec == null)
throw new ArgumentNullException("rvec");
if (tvec == null)
throw new ArgumentNullException("tvec");
//IntPtr distCoeffsPtr = ToPtr(distCoeffs);
unsafe
{
const int maxIter = 20;
double[] ar = new double[9] { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
double[] MM = new double[9],
U = new double[9],
V = new double[9],
W = new double[3];
double* param = stackalloc double[6];
CvMat matA = new CvMat(3, 3, MatrixType.F64C1);
CvMat _Ar = new CvMat(3, 3, MatrixType.F64C1, ar);
CvMat matR = new CvMat(3, 3, MatrixType.F64C1);
CvMat _r = new CvMat(3, 1, MatrixType.F64C1, new IntPtr(param));
CvMat _t = new CvMat(3, 1, MatrixType.F64C1, new IntPtr(param + 3));
CvMat _Mc = new CvMat(1, 3, MatrixType.F64C1);
CvMat _MM = new CvMat(3, 3, MatrixType.F64C1, MM);
CvMat matU = new CvMat(3, 3, MatrixType.F64C1, U);
CvMat matV = new CvMat(3, 3, MatrixType.F64C1, V);
CvMat matW = new CvMat(3, 1, MatrixType.F64C1, W);
CvMat _param = new CvMat(6, 1, MatrixType.F64C1, new IntPtr(param));
CvMat _dpdr, _dpdt;
if (!IS_MAT(objectPoints.CvPtr) ||
!IS_MAT(imagePoints.CvPtr) ||
!IS_MAT(cameraMatrix.CvPtr) ||
!IS_MAT(rvec.CvPtr) ||
!IS_MAT(tvec.CvPtr))
{
throw new ArgumentException();
}
int count = Math.Max(objectPoints.Cols, objectPoints.Rows);
CvMat matM = new CvMat(1, count, MatrixType.F64C3);
CvMat _m = new CvMat(1, count, MatrixType.F64C2);
ConvertPointsHomogeneous(objectPoints, matM);
ConvertPointsHomogeneous(imagePoints, _m);
Convert(cameraMatrix, matA);
if (!((rvec.ElemType == MatrixType.F64C1 || rvec.ElemType == MatrixType.F32C1) &&
(rvec.Rows == 1 || rvec.Cols == 1) && rvec.Rows * rvec.Cols * rvec.ElemChannels == 3))
{
throw new ArgumentException();
}
if (!((tvec.ElemType == MatrixType.F64C1 || tvec.ElemType == MatrixType.F32C1) &&
(tvec.Rows == 1 || tvec.Cols == 1) && tvec.Rows * tvec.Cols * tvec.ElemChannels == 3))
{
throw new ArgumentException();
}
CvMat _mn = new CvMat(1, count, MatrixType.F64C2);
CvMat _Mxy = new CvMat(1, count, MatrixType.F64C2);
// normalize image points
// (unapply the intrinsic matrix transformation and distortion)
UndistortPoints_(_m, _mn, matA, distCoeffs, null, _Ar);
if (useExtrinsicGuess)
{
using (CvMat _r_temp = new CvMat(rvec.Rows, rvec.Cols, MatrixType.F64C1))
using (CvMat _t_temp = new CvMat(tvec.Rows, tvec.Cols, MatrixType.F64C1))
{
Convert(rvec, _r_temp);
Convert(tvec, _t_temp);
for (int i = 0; i < Math.Max(rvec.Rows, rvec.Cols); i++)
{
param[i] = _r_temp.GetReal1D(i);
param[i + 3] = _t_temp.GetReal1D(i);
}
}
}
else
{
CvScalar Mc = Avg(matM);
_Mc[0] = Mc.Val0;
_Mc[1] = Mc.Val1;
_Mc[2] = Mc.Val2;
Reshape(matM, matM, 1, count);
MulTransposed(matM, _MM, true, _Mc);
SVD(_MM, matW, null, matV, SVDFlag.ModifyA | SVDFlag.V_T);
// initialize extrinsic parameters
if (W[2] / W[1] < 1e-3 || count < 4)
{
// a planar structure case (all M's lie in the same plane)
double[] h = new double[9];
CvMat R_transform = matV;
CvMat T_transform = new CvMat(3, 1, MatrixType.F64C1);
CvMat matH = new CvMat(3, 3, MatrixType.F64C1, h);
CvMat _h1, _h2, _h3;
if (V[2] * V[2] + V[5] * V[5] < 1e-10)
SetIdentity(R_transform);
if (Det(R_transform) < 0)
Scale(R_transform, R_transform, -1);
//GEMM(R_transform, _Mc, -1, null, 0, T_transform, GemmOperation.B_T);
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 1; c++)
{
double sum = 0;
for (int k = 0; k < 3; k++)
{
sum += R_transform.GetReal2D(r, k) * _Mc.GetReal2D(c, k);
}
T_transform.SetReal2D(r, c, sum * -1);
}
}
for (int i = 0; i < count; i++)
{
double* Rp = R_transform.DataDouble;
double* Tp = T_transform.DataDouble;
double* src = matM.DataDouble + i * 3;
double* dst = _Mxy.DataDouble + i * 2;
dst[0] = Rp[0] * src[0] + Rp[1] * src[1] + Rp[2] * src[2] + Tp[0];
dst[1] = Rp[3] * src[0] + Rp[4] * src[1] + Rp[5] * src[2] + Tp[1];
}
FindHomography_(_Mxy, _mn, matH);
GetCol(matH, out _h1, 0);
GetCol(matH, out _h2, 0);
GetCol(matH, out _h3, 0);
_h2.DataDouble += 1;
_h3.DataDouble += 2;
double h1_norm = Math.Sqrt(h[0] * h[0] + h[3] * h[3] + h[6] * h[6]);
double h2_norm = Math.Sqrt(h[1] * h[1] + h[4] * h[4] + h[7] * h[7]);
Scale(_h1, _h1, 1.0 / h1_norm);
Scale(_h2, _h2, 1.0 / h2_norm);
Scale(_h3, _t, 2.0 / (h1_norm + h2_norm));
CrossProduct(_h1, _h2, _h3);
Rodrigues2_(matH, _r);
Rodrigues2_(_r, matH);
MatMulAdd(matH, T_transform, _t, _t);
MatMul(matH, R_transform, matR);
Rodrigues2_(matR, _r);
}
else
{
// non-planar structure. Use DLT method
double[] LL = new double[12 * 12],
LW = new double[12],
LV = new double[12 * 12];
CvMat _LL = new CvMat(12, 12, MatrixType.F64C1, LL);
CvMat _LW = new CvMat(12, 1, MatrixType.F64C1, LW);
CvMat _LV = new CvMat(12, 12, MatrixType.F64C1, LV);
CvMat _RR, _tt;
CvPoint3D64f* M = (CvPoint3D64f*)matM.DataDouble;
CvPoint2D64f* mn = (CvPoint2D64f*)_mn.DataDouble;
CvMat matL = new CvMat(2 * count, 12, MatrixType.F64C1);
double* L = matL.DataDouble;
for (int i = 0; i < count; i++, L += 24)
{
double x = -mn[i].X, y = -mn[i].Y;
L[0] = L[16] = M[i].X;
L[1] = L[17] = M[i].Y;
L[2] = L[18] = M[i].Z;
L[3] = L[19] = 1.0;
L[4] = L[5] = L[6] = L[7] = 0.0;
L[12] = L[13] = L[14] = L[15] = 0.0;
L[8] = x * M[i].X;
L[9] = x * M[i].Y;
L[10] = x * M[i].Z;
L[11] = x;
L[20] = y * M[i].X;
L[21] = y * M[i].Y;
L[22] = y * M[i].Z;
L[23] = y;
}
MulTransposed(matL, _LL, true);
SVD(_LL, _LW, null, _LV, SVDFlag.ModifyA | SVDFlag.V_T);
double[] LV12 = new double[12];
Array.Copy(LV, 11 * 12, LV12, 0, 12);
CvMat _RRt = new CvMat(3, 4, MatrixType.F64C1, LV12);
GetCols(_RRt, out _RR, 0, 3);
GetCol(_RRt, out _tt, 3);
if (Det(_RR) < 0)
Scale(_RRt, _RRt, -1);
double sc = Norm(_RR);
SVD(_RR, matW, matU, matV, SVDFlag.ModifyA | SVDFlag.U_T | SVDFlag.V_T);
GEMM(matU, matV, 1, null, 0, matR, GemmOperation.A_T);
Scale(_tt, _t, Norm(matR) / sc);
Rodrigues2_(matR, _r);
}
}
Cv.Reshape(matM, matM, 3, 1);
Cv.Reshape(_mn, _mn, 2, 1);
// refine extrinsic parameters using iterative algorithm
CvLevMarq solver = new CvLevMarq(6, count * 2, new CvTermCriteria(maxIter, float.Epsilon), true);
Copy(_param, solver.Param);
/*
Console.WriteLine("matM-----");
for (int i = 0; i < matM.Rows * matM.Cols; i++)
{
Console.WriteLine("{0}\t", matM[i].Val0);
}
Console.WriteLine("_mn-----");
for (int i = 0; i < _mn.Rows * _mn.Cols; i++)
{
Console.WriteLine(_mn[i].Val0);
}
Console.WriteLine("_param-----");
for (int i = 0; i < _param.Rows * _param.Cols; i++)
{
Console.WriteLine(_param[i].Val0);
}*/
for (; ; )
{
CvMat matJ, _err, __param;
bool proceed = solver.Update(out __param, out matJ, out _err);
Copy(__param, _param);
if (!proceed || _err == null)
break;
Reshape(_err, _err, 2, 1);
if (matJ != null)
{
GetCols(matJ, out _dpdr, 0, 3);
GetCols(matJ, out _dpdt, 3, 6);
ProjectPoints2(matM, _r, _t, matA, distCoeffs,
_err, _dpdr, _dpdt, null, null, null);
}
else
{
ProjectPoints2(matM, _r, _t, matA, distCoeffs, _err, null, null, null, null, null);
}
Sub(_err, _m, _err);
Reshape(_err, _err, 1, 2 * count);
}
Copy(solver.Param, _param);
for (int i = 0; i < 3; i++)
{
rvec.SetReal1D(i, param[i]);
tvec.SetReal1D(i, param[i + 3]);
}
}
}
// affect a CvMat in an unspeakable way (maybe this: http://ieeexplore.ieee.org/document/4062288/?reload=true)
// FIX : looks like there's lots of room for optimization
// NOTE : It doesn't just look like it, IT'S P.A.N.A.R.G.O.
// <= returns 32bit float greyscale
static public CvMat IBO(CvMat image)
{
int imageRows = image.Rows;
int imageCols = image.Cols;
CvMat IBOsub = new CvMat(imageRows, imageCols, MatrixType.F32C1, new CvScalar(0));
const int kernelCols = 3;
const int kernelRows = 3;
int x, y, k, l;
bool firstElement;
int a1, b1, a2, b2;
a1 = (kernelCols - 1) / 2;
b1 = a1 + 1;
a2 = (kernelRows - 1) / 2;
b2 = a2 + 1;
//convert g(x,y) = Z = Ð(f(x,y)) for 8 values surrounding the x,y value...
for (x = a1; x < imageCols - a1; x++)
{
for (y = a1; y < imageRows - a1; y++)
{
firstElement = true;
for (k = x - a1; k < x + b1; k++)
{
for (l = y - a2; l < y + b2; l++)
{
double val = image.GetReal2D(l, k);
if (firstElement)
{
IBOsub.SetReal2D(y, x, val); // * image.at<float>(l,k);
firstElement = false;
}
else
{
IBOsub.SetReal2D(y, x, IBOsub.GetReal2D(y, x) * val); // originally there was multiplication not addition
// TODO : I changed back to multiplication, because addition gave back white image. Great?
}
}
}
}
}
// TODO : I subtracted this addition because it seemed too much. Good?
////////this is an addition by us
#if (false)
{
for (x = a1; x < imageCols - a1; x++)
{
for (y = a1; y < imageRows - a1; y++)
{
double sqr = IBOsub.GetReal2D(y, x);
sqr *= sqr;
IBOsub.SetReal2D(y, x, sqr);
}
}
}
#endif
for (x = 0; x < imageCols; x++)
{
for (k = 0; k < a1; k++)
{
IBOsub.SetReal2D(k, x, IBOsub.GetReal2D(a1, x));
IBOsub.SetReal2D(imageRows - (k + 1), x, IBOsub.GetReal2D(imageRows - (a1 + 1), x));
}
}
for (y = 0; y < imageRows; y++)
{
for (k = 0; k < a2; k++)
{
IBOsub.SetReal2D(y, k, IBOsub.GetReal2D(y, a2));
IBOsub.SetReal2D(y, imageCols - (k + 1), IBOsub.GetReal2D(y, imageCols - (a2 + 1)));
}
}
// find the max value of the mat
double minVal, maxVal;
CvPoint minLoc, maxLoc;
Cv.MinMaxLoc(IBOsub, out minVal, out maxVal, out minLoc, out maxLoc);
image = (IBOsub * (1.0 / maxVal)); // by this function, image is now a new object
IplConvKernel element = new IplConvKernel(3, 3, 1, 1, ElementShape.Rect); // simply returns a predefined shape-in-a-Mat
Cv.Dilate(image, image, element);
return(image);
}
// NOTE : "mask" is considered to be all white
// => image must be 8bit greyscale
// <= returns an int
static public int NeighborhoodValleyEmphasis(CvMat image)
{
int i, r, c, g, t;
double temp;
double[] gray_valueV = new double[256]; // initiallized to 0
double[] p0t = new double[256]; // initiallized to 0
double[] p1t = new double[256]; // initiallized to 0
double[] m0t = new double[256]; // initiallized to 0
double[] m1t = new double[256]; // initiallized to 0
// NOTE : removed loop with
for (r = image.Rows - 1; r >= 0; --r)
{
for (c = image.Cols - 1; c >= 0; --c)
{
// mask is all white, removed check: "if (mask.at<uchar>( r, c ) == 255)"
++gray_valueV[(int)image.GetReal2D(r, c)];
}
}
int numOfPixels = image.Rows * image.Cols;
for (r = gray_valueV.Length - 1; r >= 0; --r)
{
gray_valueV[r] /= numOfPixels; //The probability of occurrence of gray level i
}
// We will find p0(t) and p1(t) for each gray level probability of the 2 classes
for (t = 0; t < 256; t++)
{
for (g = 0; g < t; g++)
{
p0t[t] += gray_valueV[g];
}
for (g = t; g < 256; g++)
{
p1t[t] += gray_valueV[g];
}
}
for (t = 0; t < 256; t++)
{
for (g = 0; g < t; g++)
{
if (p0t[t] != 0)
{
m0t[t] += g * gray_valueV[g] / p0t[t];
}
}
for (g = t; g < 256; g++)
{
if (p1t[t] != 0)
{
m1t[t] += g * gray_valueV[g] / p1t[t];
}
}
}
double[] sigma_b2 = new double[256];
for (t = 0; t < 256; t++)
{
sigma_b2[t] = (p0t[t] * m0t[t] * m0t[t] + p1t[t] * m1t[t] * m1t[t]);
}
double[] neighborhood_valV = new double[256];
i = 11; // how's "11" chosen?
for (t = 0; t < i; t++)
{
temp = 0;
for (g = 1; g <= i; g++) //The i values greater than t
{
temp += gray_valueV[g + t];
}
for (g = 0; g <= i; g++) //The first i values
{
temp += gray_valueV[g];
}
neighborhood_valV[t] = temp;
}
for (t = i; t < 256 - i; t++)
{
temp = 0;
for (g = 1; g < i; g++)
{
temp += gray_valueV[t + g];
temp += gray_valueV[t - g];
}
temp += gray_valueV[t];
neighborhood_valV[t] = temp;
}
for (t = 256 - i; t < 256; t++)
{
temp = 0;
for (g = 1; g <= i; g++) //The i values less than t
{
temp += gray_valueV[t - g];
}
for (g = 0; g <= i; g++) //The last i values
{
temp += gray_valueV[255 - g];
}
neighborhood_valV[t] = temp;
}
// t is the OTSU threshold
// No need to sort values, just keep index of max OTSU value, that's all! was: "std::map<double, int, std::greater<double>> threshM;"
double maxValue = double.NegativeInfinity;
int maxValueIndex = -1;
for (t = 0; t < 256; ++t)
{
double newValue = (1 - neighborhood_valV[t]) * sigma_b2[t];
if (newValue > maxValue)
{
maxValue = newValue;
maxValueIndex = t;
}
}
//t is the Neighborhood Valley-emphasis method threshold
return(maxValueIndex);
}
// 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);
}
}
