/// <summary>
/// Point to a specified star
/// </summary>
/// <param name="star">the target star</param>
public void PointTo(Star star)
{
/*
* v: up
* u: right
* n: back
*/
if (star.Dec > 90 - 1e-9)
{
N = new MCvPoint3D32f(0, -1, 0);
V = new MCvPoint3D32f(0, 0, 1);
U = new MCvPoint3D32f(1, 0, 0);
return;
}
var starPos = star.Position.GetNormalizedPoint();
starPos.x = -starPos.x; starPos.y = -starPos.y; starPos.z = -starPos.z;
N = starPos;
var up = new MCvPoint3D32f(0, 1, 0);
U = N.CrossProduct(up).GetNormalizedPoint();
V = U.CrossProduct(N).GetNormalizedPoint();
}
/// <summary>
/// Perform a search within the given radius
/// </summary>
/// <param name="position">The center of the search area</param>
/// <param name="radius">The radius of the search</param>
/// <param name="maxResults">The maximum number of results to return</param>
/// <returns>The neighbors found</returns>
public Neighbor[] RadiusSearch(MCvPoint3D32f position, double radius, int maxResults)
{
_query.Data[0, 0] = position.X;
_query.Data[0, 1] = position.Y;
_query.Data[0, 2] = position.Z;
using (Mat indicies = new Mat(new Size(maxResults, 1), DepthType.Cv32S, 1))
using (Mat sqrDistances = new Mat(new Size(maxResults, 1), DepthType.Cv32F, 1))
{
indicies.SetTo(new MCvScalar(-1));
sqrDistances.SetTo(new MCvScalar(-1));
_flannIndex.RadiusSearch(_query, indicies, sqrDistances, radius, maxResults);
int[] indiciesVal = new int[indicies.Rows * indicies.Cols];
indicies.CopyTo(indiciesVal);
float[] sqrDistancesVal = new float[sqrDistances.Rows * sqrDistances.Cols];
sqrDistances.CopyTo(sqrDistancesVal);
List <Neighbor> neighbors = new List <Neighbor>();
for (int j = 0; j < maxResults; j++)
{
if (indiciesVal[j] <= 0)
{
break;
}
Neighbor n = new Neighbor();
n.Index = indiciesVal[j];
n.SquareDist = sqrDistancesVal[j];
neighbors.Add(n);
}
return(neighbors.ToArray());
}
}
public CalibrationUtil(Camera cam)
{
this.cam = cam;
objectPoints = new MCvPoint3D32f[1][];
objectPoints[0] = new MCvPoint3D32f[0];
imagePoints = new PointF[1][];
imagePoints[0] = new PointF[0];
intrinsics = new IntrinsicCameraParameters();
imageSize = new Size(Screen.width, Screen.height);
//Settings based on Mapamok default settings
calibrationType = CALIB_TYPE.CV_CALIB_USE_INTRINSIC_GUESS
| CALIB_TYPE.CV_CALIB_FIX_PRINCIPAL_POINT //required to work properly !!
| CALIB_TYPE.CV_CALIB_FIX_ASPECT_RATIO
| CALIB_TYPE.CV_CALIB_FIX_K1
| CALIB_TYPE.CV_CALIB_FIX_K2
| CALIB_TYPE.CV_CALIB_FIX_K3
| CALIB_TYPE.CV_CALIB_FIX_K4
| CALIB_TYPE.CV_CALIB_FIX_K5
| CALIB_TYPE.CV_CALIB_ZERO_TANGENT_DIST
;
termCriteria = new MCvTermCriteria();
}
/// <summary>
/// Retrieve all the points (x, y, z position in meters) from Kinect, row by row.
/// </summary>
/// <returns>All the points (x, y, z position in meters) from Kinect, row by row.</returns>
public MCvPoint3D32f[] RetrievePointCloudMap()
{
IntPtr img = CvInvoke.cvRetrieveFrame(Ptr, (int)OpenNIDataType.PointCloudMap);
if (img == IntPtr.Zero)
{
return(null);
}
if (FlipType != Emgu.CV.CvEnum.FLIP.NONE)
{
CvInvoke.cvFlip(img, img, FlipType);
}
MIplImage iplImage = (MIplImage)Marshal.PtrToStructure(img, typeof(MIplImage));
MCvPoint3D32f[] points = new MCvPoint3D32f[iplImage.width * iplImage.height];
GCHandle handle = GCHandle.Alloc(points, GCHandleType.Pinned);
using (Matrix <float> m = new Matrix <float>(iplImage.height, iplImage.width, handle.AddrOfPinnedObject()))
{
CvInvoke.cvCopy(img, m, IntPtr.Zero);
}
handle.Free();
return(points);
}
public MCvPoint3D32f Kalman4Joints(float x, float y, float z, Kalman kalman, SyntheticData syntheticData)
{
//Kalman headPC1
Emgu.CV.Matrix <float> MatrixGet = new Emgu.CV.Matrix <float>(6, 1);
MatrixGet[0, 0] = x;
MatrixGet[1, 0] = y;
MatrixGet[2, 0] = z;
kalman = new Kalman(6, 3, 0);
Emgu.CV.Matrix <float> state = MatrixGet;
kalman.CorrectedState = state;
kalman.TransitionMatrix = syntheticData.transitionMatrix;
kalman.MeasurementNoiseCovariance = syntheticData.measurementNoise;
kalman.ProcessNoiseCovariance = syntheticData.processNoise;
kalman.ErrorCovariancePost = syntheticData.errorCovariancePost;
kalman.MeasurementMatrix = syntheticData.measurementMatrix;
Matrix <float> prediction = new Matrix <float>(3, 1);
prediction = kalman.Predict();
MCvPoint3D32f predictPointheadPC1 = new MCvPoint3D32f(prediction[0, 0], prediction[1, 0], prediction[2, 0]);
MCvPoint3D32f measurePointheadPC1 = new MCvPoint3D32f(syntheticData.GetMeasurement()[0, 0],
syntheticData.GetMeasurement()[1, 0], syntheticData.GetMeasurement()[2, 0]);
Matrix <float> estimated = kalman.Correct(syntheticData.GetMeasurement());
MCvPoint3D32f estimatedPoint = new MCvPoint3D32f(estimated[0, 0], estimated[1, 0], estimated[2, 0]);
syntheticData.GoToNextState();
return(estimatedPoint);
}
public void Calibrate()
{
MCvPoint3D32f[][] cornerPointsOfImages = new MCvPoint3D32f[this.AcceptedImagesCount][];
for (int i = 0; i < this.AcceptedImagesCount; i++)
{
// for each captured image the physical coordinates of the corner points of the chess board are the same
// in the coordinate system of the chess board.
cornerPointsOfImages[i] = this.ChessBoard.CornerPoints;
}
IntrinsicCameraParameters intrinsicCameraParameters = new IntrinsicCameraParameters();
// We initialize the intrinsic matrix such that the two focal lengths have a ratio of 1.0
intrinsicCameraParameters.IntrinsicMatrix[0, 0] = 1.0;
intrinsicCameraParameters.IntrinsicMatrix[1, 1] = 1.0;
ExtrinsicCameraParameters[] extrinsicCameraParametersForImages; // will hold the rotation and translation for each captured chess board
CameraCalibration.CalibrateCamera(
cornerPointsOfImages,
m_AcceptedImageCorners.ToArray(),
this.OriginalImage.Size,
intrinsicCameraParameters,
Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_FIX_ASPECT_RATIO | Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_FIX_K3 | Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_ZERO_TANGENT_DIST,
out extrinsicCameraParametersForImages);
m_IntrinsicCameraParameters = intrinsicCameraParameters;
this.State = CalibratorState.Calibrated;
}
private static double ComputeAngleBetweenCameraNormAndPlaneNorm(VectorOfPoint3D32F trackedFeatures3D, Matrix <double> normal, VectorOfFloat raux, VectorOfFloat taux)
{
var tvec = taux.ToArray().Select(i => (double)i).ToArray();
var rotationMat = new Mat();
CvInvoke.Rodrigues(raux, rotationMat);
var rotationMatrix = new Matrix <double>(rotationMat.Rows, rotationMat.Cols, rotationMat.DataPointer);
// ???
Utils.Negotiate(ref rotationMatrix);
var cameraPosition = rotationMatrix * new Matrix <double>(tvec);
var cameraPositionPoint = new MCvPoint3D32f((float)cameraPosition[0, 0], (float)cameraPosition[1, 0], (float)cameraPosition[2, 0]);
var cameraVector = trackedFeatures3D[0] - cameraPositionPoint;
Func <double, double> radianToDegree = angle => angle * (180.0 / Math.PI);
double dotProduct = new double[] { cameraVector.X, cameraVector.Y, cameraVector.Z }.Dot(new[] { normal[0, 0], normal[0, 1], normal[0, 2] });
double acos = Math.Acos(dotProduct);
double anglResult = radianToDegree(acos);
Console.WriteLine($"Normal: [{normal.Data[0, 0]}, {normal.Data[0, 1]}, {normal.Data[0, 2]}]");
Console.WriteLine($"Angle: {anglResult}");
Console.WriteLine($"Dot product: {dotProduct}");
return(anglResult);
}
private VectorOfPoint3D32F GetPoints3d(string name)
{
var points = new List <MCvPoint3D32f>();
using (
var reader = File.OpenText(
$@"{TestCaseTestProjectPath}\csharplogs\{name}"))
{
string line;
while ((line = reader.ReadLine()) != null)
{
line = line.Remove(line.Length - 1, 1);
if (line.Contains("["))
{
line = line.Remove(0, 1);
}
if (line.Contains(";"))
{
line = line.Replace(";", "");
}
var items = line.Split(',');
float x = float.Parse(items[0]);
float y = float.Parse(items[1]);
float z = float.Parse(items[2]);
var point = new MCvPoint3D32f(x, y, z);
points.Add(point);
}
}
return(new VectorOfPoint3D32F(points.ToArray()));
}
public double Calibrate(Size resolution)
{
MCvPoint3D32f[][] objectPoints = new MCvPoint3D32f[1][];
PointF[][] imagePoints = new PointF[1][];
int count = this.FObjectPoints.Count;
objectPoints[0] = new MCvPoint3D32f[count];
imagePoints[0] = new PointF[count];
for (int i = 0; i < count; i++)
{
objectPoints[0][i] = FObjectPoints[i];
imagePoints[0][i] = FImagePoints[i];
}
IntrinsicCameraParameters intrinsicParam = new IntrinsicCameraParameters();
ExtrinsicCameraParameters[] extrinsicParams;
Matrix<double> mat = intrinsicParam.IntrinsicMatrix;
mat[0, 0] = resolution.Width;
mat[1, 1] = resolution.Height;
mat[0, 2] = resolution.Width / 2.0d;
mat[1, 2] = resolution.Height / 2.0d;
mat[2, 2] = 1;
CALIB_TYPE flags;
flags = CALIB_TYPE.CV_CALIB_FIX_K1 | CALIB_TYPE.CV_CALIB_FIX_K2 | CALIB_TYPE.CV_CALIB_FIX_K3 | CALIB_TYPE.CV_CALIB_FIX_K4 | CALIB_TYPE.CV_CALIB_FIX_K5 | CALIB_TYPE.CV_CALIB_FIX_K6 | CALIB_TYPE.CV_CALIB_USE_INTRINSIC_GUESS | CALIB_TYPE.CV_CALIB_ZERO_TANGENT_DIST;
double error = CameraCalibration.CalibrateCamera(objectPoints, imagePoints, resolution, intrinsicParam, flags, out extrinsicParams);
this.FIntrinsics = new Intrinsics(intrinsicParam, resolution);
this.FExtrinsics = new Extrinsics(extrinsicParams[0]);
return error;
}
/// <summary>
/// Project the point according to camera position.
/// </summary>
/// <param name="p">Point position, in WC</param>
/// <returns>pixel position</returns>
public PointF?Project(MCvPoint3D32f p)
{
//return new PointF(p.x, p.y);
// Extend p
Matrix <double> p_ex = new Matrix <double>(3, 1);
p_ex[0, 0] = p.x;
p_ex[1, 0] = p.y;
p_ex[2, 0] = p.z;
var rst = HomographMatrix * p_ex;
double x = rst[0, 0] / rst[2, 0];
double y = rst[1, 0] / rst[2, 0];
float angle_rad = (float)(FoV / 2 / (180 / Math.PI));
if (Math.Abs(x) > Math.Tan(angle_rad) || Math.Abs(y) > Math.Tan(angle_rad))
{
return(null);
}
else
{
return(new PointF((float)x * focalLength * scale, (float)y * focalLength * scale));
}
}
/// <summary>
/// Otvorí obrázky s šachovnicami a extrahuje rohové body
/// </summary>
/// <param name="fileList">zoznam mien obrázkov s šachovnicami</param>
/// <param name="boardSize">počet vnútorných rohov šachovnice (x-1, y-1)</param>
/// <returns></returns>
private int AddChessboardPoints(List <string> fileList, Size boardSize)
{
//PointF[][] imageCorners = new PointF[Frame_array_buffer.Length][];
//body na šachovnici
//PointF[] imageCorners;
//Emgu.CV.IOutputArray imageCorners;
//poloha rohov šachovnice v 3D priestore
MCvPoint3D32f[] objectCorners = new MCvPoint3D32f[boardSize.Height * boardSize.Width];
//3D Scene Points:
//Inicializácia vnútorných rohov šachovnice v 3D priestore (x,y,z) = (i,j,0)
for (int i = 0; i < boardSize.Height; i++)
{
for (int j = 0; j < boardSize.Width; j++)
{
objectCorners[i * boardSize.Width + j] = new MCvPoint3D32f(i, j, 0.0f);
}
}
//2D body obrázka:
Image <Gray, Byte> image; //obrázok pre načítavanie obrázka so šachovnicou
int successes = 0; //počet najdenych obrazkov so sachovnicou
//List<VectorOfPointF> corners = new List<VectorOfPointF>();
GC.Collect();
//pre všetky vstupné obrázky - uhly pohľadu
for (int i = 0; i < fileList.Count; i++)
{
var cornerPoints = new VectorOfPointF(); //vektor rohových bodov šachovnice
image = new Image <Gray, Byte>(fileList[i]); //načítaj obrázok zo zoznamu
//imageCorners = null; //CameraCalibration.FindChessboardCorners(image, boardSize, CALIB_CB_TYPE.DEFAULT);
CvInvoke.FindChessboardCorners(image, boardSize, cornerPoints, CalibCbType.Default); //získaj rohové body šachovnice
if (cornerPoints == null)
{
continue; //keď v aktuálnom obrázku nenašiel žiadne body, zoberie ďalší //imageCorners
}
//corners.Add(cornerPoints);
//image.FindCornerSubPix( imageCorners, new Size(11, 11), new Size(-1, -1), new MCvTermCriteria(30, 0.1));
//získaj rohové body so subpixelovou presnosťou
CvInvoke.CornerSubPix(image, cornerPoints, new Size(11, 11), new Size(-1, -1), new MCvTermCriteria(30, 0.1));
//CvInvoke.cvFindCornerSubPix(image, imageCorners,
// boardSize.Height * boardSize.Width,
// new Size(5, 5), new Size(-1, -1),
// new MCvTermCriteria(30, 0.1));
//keď našiel na obrázku dosť bodov (9*6), tak ich pridá do zoznamu
if (cornerPoints.Size == boardSize.Height * boardSize.Width) //imageCorners.Length
{
//zavolá metódu na pridanie bodov do zoznamov
AddPoints(cornerPoints.ToArray(), objectCorners);
successes++;
}
}
return(successes);
}
/// <summary>
/// 点云图片
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void button3_Click(object sender, EventArgs e)
{
PointF[] points = new PointF[4];
points[0].X = Convert.ToSingle(textBox1.Text);
points[0].Y = Convert.ToSingle(textBox2.Text);
points[1].X = Convert.ToSingle(textBox4.Text);
points[1].Y = Convert.ToSingle(textBox3.Text);
points[2].X = Convert.ToSingle(textBox6.Text);
points[2].Y = Convert.ToSingle(textBox5.Text);
points[3].X = Convert.ToSingle(textBox8.Text);
points[3].Y = Convert.ToSingle(textBox7.Text);
Image <Xyz, byte> pointCloudImage = new Image <Xyz, byte>(image.Width, image.Height);
MCvPoint3D32f[] realPoint = new MCvPoint3D32f[4];
Mat cameraMatrix = new Mat();
Mat distortionCoffe = new Mat();
Mat rotationMatrix = new Mat();
Mat translationMatrix = new Mat();
// CvInvoke.CalibrateCamera(realPoint,points, image.Size, cameraMatrix, distortionCoffe, rotationMatrix, translationMatrix, Emgu.CV.CvEnum.CalibType.Default, new MCvTermCriteria());
//CvInvoke.StereoRectify(,,,,,,,,,,,, Emgu.CV.CvEnum.StereoRectifyType.Default,,,,)
CvInvoke.ReprojectImageTo3D(image.Convert <Gray, byte>(), pointCloudImage, reflection, false, Emgu.CV.CvEnum.DepthType.Default);
imageBox.Image = pointCloudImage;
}
public static ExtrinsicCameraParameters Calibrate(PointF[] locals, MCvPoint3D32f[] globals, Size pattern)
{
PointF[] pl = new PointF[]
{
locals[0 + (pattern.Height -1) * pattern.Width],
locals[0 + 0 * pattern.Width],
locals[(pattern.Width-1) + (pattern.Height -1) * pattern.Width],
};
MCvPoint3D32f[] pg = new MCvPoint3D32f[]
{
globals[0 + (pattern.Height -1) * pattern.Width],
globals[0 + 0 * pattern.Width],
globals[(pattern.Width-1) + (pattern.Height -1) * pattern.Width],
};
//f2 = (p1 - p0).Normalize(1);
//f1 = (p2 - p0).Normalize(1);
//f1 = (f1 - (f1.DotProduct(f2) * f2)).Normalize(1);
//f3 = new DenseVector(new double[] { f1[1] * f2[2] - f1[2] * f2[1], f1[2] * f2[0] - f1[0] * f2[2], f1[0] * f2[1] - f1[1] * f2[0] });
//f3 = f3.Normalize(1);
var plv = pl.Select(row => new DenseVector(new double[] { row.X, row.Y })).ToArray();
return null;
}
public void CalibrateExtrinsics(PointF[] imagePoints, MCvPoint3D32f[] worldPoints)
{
if (Intrinsics == null)
throw new Exception("Intrinsics of camera are still unknown, unable to calibrate extrinsic paramters.");
Extrinsics = CameraCalibration.FindExtrinsicCameraParams2(worldPoints, imagePoints, Intrinsics);
InitializeWorldAndViewMatrices();
BoundingFrustum = new BoundingFrustum(View * Projection);
using (var img = new Image<Bgr, byte>(ImageWidth, ImageHeight))
{
foreach (var p in imagePoints)
img.Draw(new Cross2DF(p, 20, 20), new Bgr(255, 0, 255), 1);
var projectedCorners = CameraCalibration.ProjectPoints(worldPoints, Extrinsics, Intrinsics);
foreach (var p in projectedCorners)
img.Draw(new Cross2DF(p, 6, 6), new Bgr(255, 255, 0), 1);
var und = Intrinsics.Undistort(img);
img.Save(Path.Combine(Global.TmpDir, "reproject.png"));
und.Save(Path.Combine(Global.TmpDir, "undistorted.png"));
}
}
static MCvPoint3D32f rot(MCvPoint3D32f pt, double[][] mat)
{
return(new MCvPoint3D32f(
mul(pt, mat[0]),
mul(pt, mat[1]),
mul(pt, mat[2])
));
}
public static MCvPoint3D32f[] GetObjectPointsCopy()
{
var ops = new MCvPoint3D32f[M * N];
for (int y = 0; y < N; ++y)
for (int x = 0; x < M; ++x)
ops[y * M + x] = new MCvPoint3D32f(Origin.X + x * Size.X, Origin.Y - (y * Size.Y), 0);
return ops;
}
/// <summary>
/// Get the item in the specific index
/// </summary>
/// <param name="index">The index</param>
/// <returns>The item in the specific index</returns>
public MCvPoint3D32f this[int index]
{
get
{
MCvPoint3D32f result = new MCvPoint3D32f();
VectorOfPoint3D32FGetItem(_ptr, index, ref result);
return(result);
}
}
/// <summary>
/// Find the approximate nearest position in 3D
/// </summary>
/// <param name="position">The position to start the search from</param>
/// <param name="squareDist">The square distance of the nearest neighbour</param>
/// <returns>The index with the nearest 3D position</returns>
public int ApproximateNearestNeighbour(MCvPoint3D32f position, out double squareDist)
{
_query.Data[0, 0] = position.x;
_query.Data[0, 1] = position.y;
_query.Data[0, 2] = position.z;
_flannIndex.KnnSearch(_query, _index, _distance, 1, 1);
squareDist = _distance.Data[0, 0];
return(_index.Data[0, 0]);
}
/// <summary>
/// Find the approximate nearest position in 3D
/// </summary>
/// <param name="position">The position to start the search from</param>
/// <param name="squareDist">The square distance of the nearest neighbour</param>
/// <returns>The index with the nearest 3D position</returns>
public int ApproximateNearestNeighbour(MCvPoint3D32f position, out double squareDist)
{
_query.Data[0, 0] = position.x;
_query.Data[0, 1] = position.y;
_query.Data[0, 2] = position.z;
_flannIndex.KnnSearch(_query, _index, _distance, 1, 1);
squareDist = _distance.Data[0, 0];
return _index.Data[0, 0];
}
/// <summary>
/// Convert the standard vector to an array of Point3D32F
/// </summary>
/// <returns>An array of Point3D32F</returns>
public MCvPoint3D32f[] ToArray()
{
MCvPoint3D32f[] res = new MCvPoint3D32f[Size];
if (res.Length > 0)
{
GCHandle handle = GCHandle.Alloc(res, GCHandleType.Pinned);
VectorOfPoint3D32FCopyData(_ptr, handle.AddrOfPinnedObject());
handle.Free();
}
return(res);
}
private void FillWandPoints()
{
for (int i = 0; i < _pointsBuffer; i++)
{
_wand3DPoints[i] = new MCvPoint3D32f[_windMarkersCount];
for (int j = 0; j < _windMarkersCount; j++)
{
_wand3DPoints[i][j] = new MCvPoint3D32f(j * _wandWidht / 2, 0, 0);
}
}
}
public static MCvPoint3D32f[] Get3dPointsArray(Matrix <float> matrix)
{
var array = new MCvPoint3D32f[matrix.Rows];
for (int i = 0; i < matrix.Rows; i++)
{
array[i] = new MCvPoint3D32f(matrix.Data[i, 0], matrix.Data[i, 1], matrix.Data[i, 2]);
}
return(array);
}
/// <summary>
/// Initialize arrays
/// </summary>
public void cameraInitialize()
{
object_corner = new MCvPoint3D32f[nImage][];
corner_count = new PointF[nImage][];
extrinsicParams = new ExtrinsicCameraParameters[nImage];
for (int i = 0; i < nImage; i++)
{
object_corner[i] = new MCvPoint3D32f[nPoints];
corner_count[i] = new PointF[nPoints];
}
}
public MCvPoint3D32f[] GetWorldPointsCopy()
{
var m = Saddles.Width;
var n = Saddles.Height;
var ops = new MCvPoint3D32f[m * n];
for (int y = 0; y < n; ++y)
for (int x = 0; x < m; ++x)
ops[y * m + x] = new MCvPoint3D32f(Board.Left + (x + 1) * Square.Width, Board.Top + (y + 1) * Square.Height, 0);
return ops;
}
/// <summary>
/// Estimates intrinsic camera parameters and extrinsic parameters for each of the views
/// </summary>
/// <param name="objectPoints">The 3D location of the object points. The first index is the index of image, second index is the index of the point</param>
/// <param name="imagePoints">The 2D image location of the points. The first index is the index of the image, second index is the index of the point</param>
/// <param name="imageSize">The size of the image, used only to initialize intrinsic camera matrix</param>
/// <param name="intrinsicParam">The intrisinc parameters, might contains some initial values. The values will be modified by this function.</param>
/// <param name="flags">Flags</param>
/// <param name="extrinsicParams">The output array of extrinsic parameters.</param>
/// <returns>The final reprojection error</returns>
public static double CalibrateCamera(
MCvPoint3D32f[][] objectPoints,
PointF[][] imagePoints,
Size imageSize,
IntrinsicCameraParameters intrinsicParam,
CvEnum.CALIB_TYPE flags,
out ExtrinsicCameraParameters[] extrinsicParams)
{
Debug.Assert(objectPoints.Length == imagePoints.Length, "The number of images for objects points should be equal to the number of images for image points");
int imageCount = objectPoints.Length;
#region get the array that represent the point counts
int[] pointCounts = new int[objectPoints.Length];
for (int i = 0; i < objectPoints.Length; i++)
{
Debug.Assert(objectPoints[i].Length == imagePoints[i].Length, String.Format("Number of 3D points and image points should be equal in the {0}th image", i));
pointCounts[i] = objectPoints[i].Length;
}
#endregion
double reprojectionError = -1;
using (Matrix<float> objectPointMatrix = ToMatrix(objectPoints))
using (Matrix<float> imagePointMatrix = ToMatrix(imagePoints))
using (Matrix<int> pointCountsMatrix = new Matrix<int>(pointCounts))
using (Matrix<double> rotationVectors = new Matrix<double>(imageCount, 3))
using (Matrix<double> translationVectors = new Matrix<double>(imageCount, 3))
{
reprojectionError = CvInvoke.cvCalibrateCamera2(
objectPointMatrix.Ptr,
imagePointMatrix.Ptr,
pointCountsMatrix.Ptr,
imageSize,
intrinsicParam.IntrinsicMatrix,
intrinsicParam.DistortionCoeffs,
rotationVectors,
translationVectors,
flags);
extrinsicParams = new ExtrinsicCameraParameters[imageCount];
IntPtr matPtr = Marshal.AllocHGlobal(StructSize.MCvMat);
for (int i = 0; i < imageCount; i++)
{
ExtrinsicCameraParameters p = new ExtrinsicCameraParameters();
CvInvoke.cvGetRow(rotationVectors.Ptr, matPtr, i);
CvInvoke.cvTranspose(matPtr, p.RotationVector.Ptr);
CvInvoke.cvGetRow(translationVectors.Ptr, matPtr, i);
CvInvoke.cvTranspose(matPtr, p.TranslationVector.Ptr);
extrinsicParams[i] = p;
}
Marshal.FreeHGlobal(matPtr);
}
return reprojectionError;
}
public static void Calibrate(string[] imgFiles, out LensParams lensParams)
{
Size patternSize = new Size(CHESS_PATTERN_WIDTH, CHESS_PATTERN_HEIGHT);
VectorOfVectorOfPoint3D32F objPoints = new VectorOfVectorOfPoint3D32F();
VectorOfVectorOfPointF imagePoints = new VectorOfVectorOfPointF();
Size imageSize = Size.Empty;
foreach (string file in imgFiles)
{
Mat img = CvInvoke.Imread(file, ImreadModes.Grayscale);
if (imageSize == Size.Empty)
{
imageSize = new Size(img.Width, img.Height);
}
//CvInvoke.Imshow("input", img);
VectorOfPointF corners = new VectorOfPointF(patternSize.Width * patternSize.Height);
bool find = CvInvoke.FindChessboardCorners(img, patternSize, corners);
if (find)
{
MCvPoint3D32f[] points = new MCvPoint3D32f[patternSize.Width * patternSize.Height];
int loopIndex = 0;
for (int i = 0; i < patternSize.Height; i++)
{
for (int j = 0; j < patternSize.Width; j++)
{
points[loopIndex++] = new MCvPoint3D32f(j, i, 0);
}
}
objPoints.Push(new VectorOfPoint3D32F(points));
imagePoints.Push(corners);
}
}
Matrix <double> K = new Matrix <double>(3, 3);
Matrix <double> D = new Matrix <double>(4, 1);
Mat rotation = new Mat();
Mat translation = new Mat();
Fisheye.Calibrate(objPoints,
imagePoints,
imageSize,
K,
D,
rotation,
translation,
Fisheye.CalibrationFlag.CheckCond,
new MCvTermCriteria(30, 0.1)
);
lensParams = new LensParams(K, D);
}
/// <summary>
/// Create a linear flann index for 3D points
/// </summary>
/// <param name="points">The IPosition3D array</param>
public Index3D(MCvPoint3D32f[] points)
{
_points = points;
_dataHandle = GCHandle.Alloc(_points, GCHandleType.Pinned);
_dataMatrix = new Matrix<float>(_points.Length, 3, _dataHandle.AddrOfPinnedObject());
_flannIndex = new Index(_dataMatrix);
_query = new Matrix<float>(1, 3);
_distance = new Matrix<float>(1, 1);
_index = new Matrix<int>(1, 1);
}
/*
* /// <summary>
* /// Find the minimum enclosing circle for the specific array of points
* /// </summary>
* /// <param name="points">The collection of points</param>
* /// <returns>The minimum enclosing circle for the array of points</returns>
* public static CircleF MinEnclosingCircle(PointF[] points)
* {
* IntPtr seq = Marshal.AllocHGlobal(StructSize.MCvContour);
* IntPtr block = Marshal.AllocHGlobal(StructSize.MCvSeqBlock);
* GCHandle handle = GCHandle.Alloc(points, GCHandleType.Pinned);
* CvInvoke.cvMakeSeqHeaderForArray(
* CvInvoke.MakeType(CvEnum.DepthType.Cv32F, 2),
* StructSize.MCvSeq,
* StructSize.PointF,
* handle.AddrOfPinnedObject(),
* points.Length,
* seq,
* block);
* PointF center;
* float radius;
* CvInvoke.cvMinEnclosingCircle(seq, out center, out radius);
*
* CvInvoke.MinEnclosingCircle(
* return new CircleF(center, radius);
* }*/
/// <summary>
/// Re-project pixels on a 1-channel disparity map to array of 3D points.
/// </summary>
/// <param name="disparity">Disparity map</param>
/// <param name="Q">The re-projection 4x4 matrix, can be arbitrary, e.g. the one, computed by cvStereoRectify</param>
/// <returns>The reprojected 3D points</returns>
public static MCvPoint3D32f[] ReprojectImageTo3D(Mat disparity, IInputArray Q)
{
Size size = disparity.Size;
MCvPoint3D32f[] points3D = new MCvPoint3D32f[size.Width * size.Height];
GCHandle handle = GCHandle.Alloc(points3D, GCHandleType.Pinned);
using (Matrix <float> pts = new Matrix <float>(size.Height, size.Width, 3, handle.AddrOfPinnedObject(), 0))
CvInvoke.ReprojectImageTo3D(disparity, pts, Q, false, CvEnum.DepthType.Cv32F);
handle.Free();
return(points3D);
}
private VectorOfPoint3D32F getChessboardCorners(float square, Size patternSize)
{
MCvPoint3D32f[] corners = new MCvPoint3D32f[patternSize.Height * patternSize.Width];
for (int i = 0; i < patternSize.Height; i++)
{
for (int j = 0; j < patternSize.Width; j++)
{
corners[i * patternSize.Width + j] = new MCvPoint3D32f(i * square, j * square, 0);
}
}
return(new VectorOfPoint3D32F(corners));
}
private MCvPoint3D32f[][] PutObjectPointsIntoArray(List <Vector3> _objectPositions, int pointsCount, int ImageNum)
{
var objectPoints = new MCvPoint3D32f[1][];
objectPoints[0] = new MCvPoint3D32f[pointsCount];
for (int i = 0; i < pointsCount; i++)
{
objectPoints[0][i] = new MCvPoint3D32f(_objectPositions[i].x, _objectPositions[i].y, _objectPositions[i].z * -1);
}
return(objectPoints);
}
public static MCvPoint3D32f[] ObjectPoints(ISpread<Vector3D> input, bool toVVVV)
{
MCvPoint3D32f[] objectPoints = new MCvPoint3D32f[input.SliceCount];
for (int i = 0; i < input.SliceCount; i++)
{
objectPoints[i].x = (float)input[i].x;
objectPoints[i].y = toVVVV ? - (float)input[i].y : (float) input[i].y;
objectPoints[i].z = toVVVV ? - (float)input[i].z : (float) input[i].z;
}
return objectPoints;
}
public static MCvPoint3D32f[] ObjectPoints(ISpread <Vector3D> input)
{
MCvPoint3D32f[] objectPoints = new MCvPoint3D32f[input.SliceCount];
for (int i = 0; i < input.SliceCount; i++)
{
objectPoints[i].x = (float)input[i].x;
objectPoints[i].y = (float)input[i].y;
objectPoints[i].z = (float)input[i].z;
}
return(objectPoints);
}
/// <summary>
/// Reproject pixels on a 1-channel disparity map to array of 3D points.
/// </summary>
/// <param name="disparity">Disparity map</param>
/// <param name="Q">The reprojection 4x4 matrix, can be arbitrary, e.g. the one, computed by cvStereoRectify</param>
/// <returns>The reprojected 3D points</returns>
public static MCvPoint3D32f[] ReprojectImageTo3D(Image <Gray, Byte> disparity, Matrix <double> Q)
{
Size size = disparity.Size;
MCvPoint3D32f[] points3D = new MCvPoint3D32f[size.Width * size.Height];
GCHandle handle = GCHandle.Alloc(points3D, GCHandleType.Pinned);
using (Matrix <float> pts = new Matrix <float>(size.Height, size.Width, 3, handle.AddrOfPinnedObject(), 0))
CvInvoke.cvReprojectImageTo3D(disparity.Ptr, pts.Ptr, Q);
handle.Free();
return(points3D);
}
private static MCvPoint3D32f[] GetBGRColorArray(Mat bgrImg)
{
MCvPoint3D32f[] colorArrayF = new MCvPoint3D32f[bgrImg.Rows * bgrImg.Cols];
GCHandle colorHandle = GCHandle.Alloc(colorArrayF, GCHandleType.Pinned);
using (Mat colorArray = bgrImg.Reshape(bgrImg.NumberOfChannels, bgrImg.Rows * bgrImg.Cols))
using (Mat colorArrayFloat = new Mat(colorArrayF.Length, 1, DepthType.Cv32F, 3, colorHandle.AddrOfPinnedObject(), 3 * 4))
{
colorArray.ConvertTo(colorArrayFloat, DepthType.Cv32F);
}
colorHandle.Free();
return(colorArrayF);
}
/// <summary>
/// Convert the standard vector to arrays of int
/// </summary>
/// <returns>Arrays of int</returns>
public MCvPoint3D32f[][] ToArrayOfArray()
{
int size = Size;
MCvPoint3D32f[][] res = new MCvPoint3D32f[size][];
for (int i = 0; i < size; i++)
{
using (VectorOfPoint3D32F v = this[i])
{
res[i] = v.ToArray();
}
}
return(res);
}
/// <summary>
/// Find the approximate nearest position in 3D
/// </summary>
/// <param name="position">The position to start the search from</param>
/// <returns>The nearest neighbor (may be an approximation, depends in the index type).</returns>
public Neighbor NearestNeighbor(MCvPoint3D32f position)
{
_query.Data[0, 0] = position.X;
_query.Data[0, 1] = position.Y;
_query.Data[0, 2] = position.Z;
_flannIndex.KnnSearch(_query, _index, _distance, 1, 1);
Neighbor n = new Neighbor();
n.Index = _index.Data[0, 0];
n.SquareDist = _distance.Data[0, 0];
return(n);
}
public static MCvPoint3D32f[][] GetObjectPointsCopy(int num, int m, int n, float w, float h)
{
MCvPoint3D32f[][] ops = new MCvPoint3D32f[num][];
ops[0] = new MCvPoint3D32f[m * n];
for (int y = 0; y < n; ++y)
for (int x = 0; x < m; ++x)
ops[0][y * m + x] = new MCvPoint3D32f(x * w, -y * h, 0);
for (int i = 1; i < num; ++i)
ops[i] = ops[0].ToArray();
return ops;
}
private static MCvPoint3D32f[] GetDisparityPoints(Mat disparityMap)
{
MCvPoint3D32f[] pointArrayF = new MCvPoint3D32f[disparityMap.Rows * disparityMap.Cols];
GCHandle colorHandle = GCHandle.Alloc(pointArrayF, GCHandleType.Pinned);
using (Mat pointArray = disparityMap.Reshape(disparityMap.NumberOfChannels, disparityMap.Rows * disparityMap.Cols))
using (Mat pointArrayMat = new Mat(pointArrayF.Length, 1, DepthType.Cv32F, 3, colorHandle.AddrOfPinnedObject(), 3 * 4))
{
pointArray.CopyTo(pointArrayMat);
}
colorHandle.Free();
return(pointArrayF);
}
private void button2_Click(object sender, EventArgs e)
{
OpenFileDialog op = new OpenFileDialog();
if (op.ShowDialog() == DialogResult.OK)
{
Mat img = new Mat(op.FileName, Emgu.CV.CvEnum.LoadImageType.AnyColor);
imageBox1.Image = img;
}
//點，线，矩形，園，橢圓
MCvPoint3D32f mCvPoint3D32F = new MCvPoint3D32f(0, 0, 0);
PointF x = new PointF(0, 0);
PointF y = new PointF(1, 1);
LineSegment2DF lineSegment2DF = new LineSegment2DF(x, y);
//顏色
Rgb rgb = new Rgb(Color.Red);
Rgb red = new Rgb(255, 0, 0);
//類型轉換
Bitmap bitmap = new Bitmap(640, 480);
Image <Bgr, byte> image = new Image <Bgr, byte>(640, 480);
Mat mat = new Mat();
op = new OpenFileDialog();
if (op.ShowDialog() == DialogResult.OK)
{
Bitmap bitmap_new = new Bitmap(op.FileName);
Image <Bgr, byte> image_new = new Image <Bgr, byte>(op.FileName);
Mat mat_new = new Mat(op.FileName, Emgu.CV.CvEnum.LoadImageType.AnyColor);
pictureBox1.Image = bitmap_new;
pictureBox1.Image = image_new.ToBitmap();
//pictureBox1.Image = mat_new.Bitmap;
pictureBox1.Image = mat_new.ToImage <Bgr, byte>().Bitmap;
imageBox1.Image = new Image <Bgr, byte>(bitmap_new);
imageBox1.Image = image_new;
imageBox1.Image = mat_new;
}
Image <Bgr, byte> img = new Image <Bgr, byte>(200, 200, new Bgr(255, 255, 255));
Rectangle rectangle = new Rectangle(new Point(80, 80), new Size(40, 40));
CircleF circleF = new CircleF(new PointF(100, 100), 40);
string str = "I LOVE EmguCV";
Point str_location = new Point(0, 30);
img.Draw(rectangle, new Bgr(0, 255, 0), 2);
img.Draw(circleF, new Bgr(0, 0, 255), 3);
img.Draw(str, str_location, Emgu.CV.CvEnum.FontFace.HersheyComplexSmall, 1, new Bgr(0, 255, 0), 3);
imageBox1.Image = img;
}
private void StereoCameraCalibration()
{
MCvPoint3D32f[][] corners_object_Points = new MCvPoint3D32f[_imageBufferLength][]; //stores the calculated size for the chessboard
Mat r = new Mat();
Mat t = new Mat();
Mat f = new Mat();
Mat e1 = new Mat();
//fill the MCvPoint3D32f with correct mesurments
for (int k = 0; k < _imageBufferLength; k++)
{
//Fill our objects list with the real world mesurments for the intrinsic calculations
List <MCvPoint3D32f> object_list = new List <MCvPoint3D32f>();
for (int i = 0; i < _boardSize.Height; i++)
{
for (int j = 0; j < _boardSize.Width; j++)
{
object_list.Add(new MCvPoint3D32f(j * _blockSize, i * _blockSize, 0.0F));
}
}
corners_object_Points[k] = object_list.ToArray();
}
MessageBox.Show("End capturing images for calibration stereo camera");
CvInvoke.StereoCalibrate(corners_object_Points, corners_points_Left, corners_points_Right, cameraMatrix1, distCoeff1,
cameraMatrix2, distCoeff2, _left.Size, r, t, e1, f, CalibType.Default, new MCvTermCriteria(0.1e5));
MessageBox.Show("Succesful calibrate stereo camera");
Rectangle rec1 = new Rectangle();
Rectangle rec2 = new Rectangle();
//Computes rectification transforms for each head of a calibrated stereo camera.
MessageBox.Show("Start Rectify");
CvInvoke.StereoRectify(cameraMatrix1, distCoeff1,
cameraMatrix2, distCoeff2,
_left.Size,
r, t,
R1, R2, P1, P2, Q,
StereoRectifyType.Default, 0,
_left.Size, ref rec1, ref rec2);
MessageBox.Show("rect1 = " + rec1 + "\nrect2 = " + rec2 + "\nQ= \n" + Q[0, 0] + " " + Q[0, 1] + " " + Q[0, 2] + " " + Q[0, 3] +
"\n" + Q[1, 0] + " " + Q[1, 1] + " " + Q[1, 2] + " " + Q[1, 3] + "\n" + Q[2, 0] + " " + Q[2, 1] + " " +
Q[2, 2] + " " + Q[2, 3] + "\n" + Q[3, 0] + " " + Q[3, 1] + " " + Q[3, 2] + " " + Q[3, 3]);
_isCalibrate = true;
_isCameraMatrixCount = true;
}
public void CalibrateIntrinsics(PointF[][] imagePoints, MCvPoint3D32f[][] worldPoints)
{
if (Intrinsics == null)
Intrinsics = new IntrinsicCameraParameters();
var calibType = CALIB_TYPE.CV_CALIB_FIX_K3 | CALIB_TYPE.CV_CALIB_ZERO_TANGENT_DIST;
ExtrinsicCameraParameters[] extrinsics;
var totalErr = CameraCalibration.CalibrateCamera(worldPoints, imagePoints, new Size(ImageWidth, ImageHeight), Intrinsics, calibType, out extrinsics);
var err = Math.Sqrt(totalErr / (imagePoints.Length + imagePoints[0].Length));
Console.WriteLine("Calibration Finished, Reprojection Error: {0}", err);
Save(IntrinsicsFile, Intrinsics, ImageWidth, ImageHeight);
}
public ChessBoard(int xCount, int yCount)
{
this.XCount = xCount;
this.YCount = yCount;
this.PatternSize = new Size(this.XCount - 1, this.YCount - 1);
this.CornerCount = this.PatternSize.Width * this.PatternSize.Height;
List<MCvPoint3D32f> cornerPoints = new List<MCvPoint3D32f>(this.CornerCount);
for (int x = 0; x < this.XCount - 1; x++)
{
for (int y = 0; y < this.YCount - 1; y++)
{
MCvPoint3D32f cornerPoint = new MCvPoint3D32f(x * 25, y * 25, 0);
cornerPoints.Add(cornerPoint);
}
}
this.CornerPoints = cornerPoints.ToArray();
}
//called when data for any output pin is requested
public void Evaluate(int SpreadMax)
{
FPinOutOutput.SliceCount = FPinInInput.SliceCount;
if (FPinInIntrinsics[0] == null || FPinInExtrinsics[0] == null)
return;
MCvPoint3D32f[] o = new MCvPoint3D32f[1];
PointF[] im = new PointF[1];
for (int i=0; i<FPinInInput.SliceCount; i++)
{
o[0].x = (float)FPinInInput[i].x;
o[0].y = (float)FPinInInput[i].y;
o[0].z = (float)FPinInInput[i].z;
im = CameraCalibration.ProjectPoints(o, FPinInExtrinsics[0], FPinInIntrinsics[0]);
FPinOutOutput[i] = new Vector2D((double)im[0].X, (double)im[0].Y);
}
}
/// <summary>
/// Given the left and right image, computer the disparity map and the 3D point cloud.
/// </summary>
/// <param name="left">The left image</param>
/// <param name="right">The right image</param>
/// <param name="outputDisparityMap">The left disparity map</param>
/// <param name="points">The 3D point cloud within a [-0.5, 0.5] cube</param>
private static void Computer3DPointsFromStereoPair(IInputArray left, IInputArray right, Mat outputDisparityMap, out MCvPoint3D32f[] points)
{
Size size;
using (InputArray ia = left.GetInputArray())
size = ia.GetSize();
using (StereoBM stereoSolver = new StereoBM())
{
stereoSolver.Compute(left, right, outputDisparityMap);
float scale = Math.Max(size.Width, size.Height);
//Construct a simple Q matrix, if you have a matrix from cvStereoRectify, you should use that instead
using (Matrix<double> q = new Matrix<double>(
new double[,] {
{1.0, 0.0, 0.0, -size.Width/2}, //shift the x origin to image center
{0.0, -1.0, 0.0, size.Height/2}, //shift the y origin to image center and flip it upside down
{0.0, 0.0, -1.0, 0.0}, //Multiply the z value by -1.0,
{0.0, 0.0, 0.0, scale}})) //scale the object's coordinate to within a [-0.5, 0.5] cube
points = PointCollection.ReprojectImageTo3D(outputDisparityMap, q);
}
}
public Camera(string path, int width, int height, PointF[] imageCorners, MCvPoint3D32f[] worldCorners)
{
Width = width;
Height = height;
Intrinsics = Markers.Chessboard.Load(path, width, height, out UndistortX, out UndistortY);
Extrinsics = CameraCalibration.FindExtrinsicCameraParams2(worldCorners, imageCorners, Intrinsics);
var ext = Extrinsics;
View = new Matrix((float)ext.ExtrinsicMatrix[0, 0], -(float)ext.ExtrinsicMatrix[1, 0], -(float)ext.ExtrinsicMatrix[2, 0], 0,
(float)ext.ExtrinsicMatrix[0, 1], -(float)ext.ExtrinsicMatrix[1, 1], -(float)ext.ExtrinsicMatrix[2, 1], 0,
(float)ext.ExtrinsicMatrix[0, 2], -(float)ext.ExtrinsicMatrix[1, 2], -(float)ext.ExtrinsicMatrix[2, 2], 0,
(float)ext.ExtrinsicMatrix[0, 3], -(float)ext.ExtrinsicMatrix[1, 3], -(float)ext.ExtrinsicMatrix[2, 3], 1);
Intrinsics.GetIntrinsicMatrixValues(width, height, 0, 0,
out FovX, out FovY,
out FocalLength, out PrincipalPoint,
out PixelAspectRatio);
World = Matrix.Invert(View);
Projection = Matrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians((float)FovY), ((float)width) / ((float)height), 0.1f, 4.0f);
BoundingFrustum = new BoundingFrustum(View * Projection);
if (Global.No)
using (var img = new Image<Bgr, byte>(@"C:\Users\Jaap\My Dropbox\Data\Pentacorn.Vision\Offline\Casio Ex S5\Tape on Melamine\CIMG0606.JPG"))
{
foreach (var p in imageCorners)
img.Draw(new Cross2DF(p, 20, 20), new Bgr(255, 0, 255), 1);
var projectedCorners = CameraCalibration.ProjectPoints(worldCorners, Extrinsics, Intrinsics);
foreach (var p in projectedCorners)
img.Draw(new Cross2DF(p, 6, 6), new Bgr(255, 255, 0), 1);
var und = Intrinsics.Undistort(img);
img.Save(@"C:\Users\Jaap\Temp\img.png");
und.Save(@"C:\Users\Jaap\Temp\und.png");
}
}
/// <summary>
/// Given the left and right image, computer the disparity map and the 3D point cloud.
/// </summary>
/// <param name="left">The left image</param>
/// <param name="right">The right image</param>
/// <param name="leftDisparityMap">The left disparity map</param>
/// <param name="points">The 3D point cloud within a [-0.5, 0.5] cube</param>
private static void Computer3DPointsFromImages(Image<Gray, Byte> left, Image<Gray, Byte> right, out Image<Gray, Int16> leftDisparityMap, out MCvPoint3D32f[] points)
{
Size size = left.Size;
using (Image<Gray, Int16> leftDisparity = new Image<Gray, Int16>(size))
using (Image<Gray, Int16> rightDisparity = new Image<Gray, Int16>(size))
using (StereoGC gc = new StereoGC(16, 2))
{
gc.FindStereoCorrespondence(left, right, leftDisparity, rightDisparity);
leftDisparityMap = leftDisparity * (-16);
float scale = Math.Max(size.Width, size.Height);
//Construct a simple Q matrix, if you have a matrix from cvStereoRectify, you should use that instead
using (Matrix<double> q = new Matrix<double>(
new double[,] {
{1.0, 0.0, 0.0, -size.Width/2}, //shift the x origin to image center
{0.0, -1.0, 0.0, size.Height/2}, //shift the y origin to image center and flip it upside down
{0.0, 0.0, 16.0, 0.0}, //Multiply the z value by 16,
{0.0, 0.0, 0.0, scale}})) //scale the object's corrdinate to within a [-0.5, 0.5] cube
points = PointCollection.ReprojectImageTo3D(leftDisparity, q);
}
}
private static Matrix<float> ToMatrix(MCvPoint3D32f[][] data)
{
int elementCount = 0;
foreach (MCvPoint3D32f[] d in data) elementCount += d.Length;
Matrix<float> res = new Matrix<float>(elementCount, 3);
Int64 address = res.MCvMat.data.ToInt64();
foreach (MCvPoint3D32f[] d in data)
{
int lengthInBytes = d.Length * StructSize.MCvPoint3D32f;
GCHandle handle = GCHandle.Alloc(d, GCHandleType.Pinned);
Emgu.Util.Toolbox.memcpy(new IntPtr(address), handle.AddrOfPinnedObject(), lengthInBytes);
handle.Free();
address += lengthInBytes;
}
return res;
}
/// <summary>
/// Build an Oct-Tree from the given points
/// </summary>
/// <param name="points">The points to be inserted into the Oct-Tree</param>
/// <param name="maxLevels">The maximum levels of the Oct-Tree</param>
/// <param name="minPoints">The minimum number of points in each level</param>
public void BuildTree(MCvPoint3D32f[] points, int maxLevels, int minPoints)
{
GCHandle handle = GCHandle.Alloc(points, GCHandleType.Pinned);
CvOctreeBuildTree(_ptr, handle.AddrOfPinnedObject(), points.Length, maxLevels, minPoints);
handle.Free();
}
/// <summary>
/// Reproject pixels on a 1-channel disparity map to array of 3D points.
/// </summary>
/// <param name="disparity">Disparity map</param>
/// <param name="Q">The reprojection 4x4 matrix, can be arbitrary, e.g. the one, computed by cvStereoRectify</param>
/// <returns>The reprojected 3D points</returns>
public static MCvPoint3D32f[] ReprojectImageTo3D(Image<Gray, Byte> disparity, Matrix<double> Q)
{
Size size = disparity.Size;
MCvPoint3D32f[] points3D = new MCvPoint3D32f[size.Width * size.Height];
GCHandle handle = GCHandle.Alloc(points3D, GCHandleType.Pinned);
using (Matrix<float> pts = new Matrix<float>(size.Height, size.Width, 3, handle.AddrOfPinnedObject(), 0))
CvInvoke.cvReprojectImageTo3D(disparity.Ptr, pts.Ptr, Q);
handle.Free();
return points3D;
}
/// <summary>
/// Get the points within the specific sphere
/// </summary>
/// <param name="center">The center of the sphere</param>
/// <param name="radius">The radius of the sphere</param>
/// <returns>The points withing the specific sphere</returns>
public MCvPoint3D32f[] GetPointsWithinSphere(MCvPoint3D32f center, float radius)
{
CvOctreeGetPointsWithinSphere(_ptr, ref center, radius, _pointSeq);
return _pointSeq.ToArray();
}
private static extern void CvOctreeGetPointsWithinSphere(IntPtr tree, ref MCvPoint3D32f center, float radius, IntPtr pointSeq);
/*
/// <summary>
/// Find the minimum enclosing circle for the specific array of points
/// </summary>
/// <param name="points">The collection of points</param>
/// <returns>The minimum enclosing circle for the array of points</returns>
public static CircleF MinEnclosingCircle(PointF[] points)
{
IntPtr seq = Marshal.AllocHGlobal(StructSize.MCvContour);
IntPtr block = Marshal.AllocHGlobal(StructSize.MCvSeqBlock);
GCHandle handle = GCHandle.Alloc(points, GCHandleType.Pinned);
CvInvoke.cvMakeSeqHeaderForArray(
CvInvoke.MakeType(CvEnum.DepthType.Cv32F, 2),
StructSize.MCvSeq,
StructSize.PointF,
handle.AddrOfPinnedObject(),
points.Length,
seq,
block);
PointF center;
float radius;
CvInvoke.cvMinEnclosingCircle(seq, out center, out radius);
CvInvoke.MinEnclosingCircle(
return new CircleF(center, radius);
}*/
/// <summary>
/// Re-project pixels on a 1-channel disparity map to array of 3D points.
/// </summary>
/// <param name="disparity">Disparity map</param>
/// <param name="Q">The re-projection 4x4 matrix, can be arbitrary, e.g. the one, computed by cvStereoRectify</param>
/// <returns>The reprojected 3D points</returns>
public static MCvPoint3D32f[] ReprojectImageTo3D(IInputArray disparity, IInputArray Q)
{
Size size;
using (InputArray ia = disparity.GetInputArray())
size = ia.GetSize();
MCvPoint3D32f[] points3D = new MCvPoint3D32f[size.Width * size.Height];
GCHandle handle = GCHandle.Alloc(points3D, GCHandleType.Pinned);
using (Matrix<float> pts = new Matrix<float>(size.Height, size.Width, 3, handle.AddrOfPinnedObject(), 0))
CvInvoke.ReprojectImageTo3D(disparity, pts, Q, false, CvEnum.DepthType.Cv32F);
handle.Free();
return points3D;
}
/// <summary>
/// Create a line segment with the specific start point and end point
/// </summary>
/// <param name="p1">The first point on the line segment</param>
/// <param name="p2">The second point on the line segment</param>
public LineSegment3DF(MCvPoint3D32f p1, MCvPoint3D32f p2)
{
_p1 = p1;
_p2 = p2;
}
/// <summary>
/// Computes projections of 3D points to the image plane given intrinsic and extrinsic camera parameters.
/// Optionally, the function computes jacobians - matrices of partial derivatives of image points as functions of all the input parameters w.r.t. the particular parameters, intrinsic and/or extrinsic.
/// The jacobians are used during the global optimization in cvCalibrateCamera2 and cvFindExtrinsicCameraParams2.
/// The function itself is also used to compute back-projection error for with current intrinsic and extrinsic parameters.
/// </summary>
/// <remarks>Note, that with intrinsic and/or extrinsic parameters set to special values, the function can be used to compute just extrinsic transformation or just intrinsic transformation (i.e. distortion of a sparse set of points) </remarks>
/// <param name="objectPoints">The array of object points.</param>
/// <param name="extrin">Extrinsic parameters</param>
/// <param name="intrin">Intrinsic parameters</param>
/// <param name="mats">Optional matrix supplied in the following order: dpdrot, dpdt, dpdf, dpdc, dpddist</param>
/// <returns>The array of image points which is the projection of <paramref name="objectPoints"/></returns>
public static PointF[] ProjectPoints(
MCvPoint3D32f[] objectPoints,
ExtrinsicCameraParameters extrin,
IntrinsicCameraParameters intrin,
params Matrix<float>[] mats)
{
PointF[] imagePoints = new PointF[objectPoints.Length];
int matsLength = mats.Length;
GCHandle handle1 = GCHandle.Alloc(objectPoints, GCHandleType.Pinned);
GCHandle handle2 = GCHandle.Alloc(imagePoints, GCHandleType.Pinned);
using (Matrix<float> pointMatrix = new Matrix<float>(objectPoints.Length, 1, 3, handle1.AddrOfPinnedObject(), 3 * sizeof(float)))
using (Matrix<float> imagePointMatrix = new Matrix<float>(imagePoints.Length, 1, 2, handle2.AddrOfPinnedObject(), 2 * sizeof(float)))
CvInvoke.cvProjectPoints2(
pointMatrix,
extrin.RotationVector.Ptr,
extrin.TranslationVector.Ptr,
intrin.IntrinsicMatrix.Ptr,
intrin.DistortionCoeffs.Ptr,
imagePointMatrix,
matsLength > 0 ? mats[0] : IntPtr.Zero,
matsLength > 1 ? mats[1] : IntPtr.Zero,
matsLength > 2 ? mats[2] : IntPtr.Zero,
matsLength > 3 ? mats[3] : IntPtr.Zero,
matsLength > 4 ? mats[4] : IntPtr.Zero);
handle1.Free();
handle2.Free();
return imagePoints;
}
/// <summary>
/// Estimates extrinsic camera parameters using known intrinsic parameters and and extrinsic parameters for each view. The coordinates of 3D object points and their correspondent 2D projections must be specified. This function also minimizes back-projection error.
/// </summary>
/// <param name="objectPoints">The array of object points</param>
/// <param name="imagePoints">The array of corresponding image points</param>
/// <param name="intrin">The intrinsic parameters</param>
/// <returns>The extrinsic parameters</returns>
public static ExtrinsicCameraParameters FindExtrinsicCameraParams2(
MCvPoint3D32f[] objectPoints,
PointF[] imagePoints,
IntrinsicCameraParameters intrin)
{
ExtrinsicCameraParameters p = new ExtrinsicCameraParameters();
GCHandle handle1 = GCHandle.Alloc(objectPoints, GCHandleType.Pinned);
GCHandle handle2 = GCHandle.Alloc(imagePoints, GCHandleType.Pinned);
using (Matrix<float> objectPointMatrix = new Matrix<float>(objectPoints.Length, 3, handle1.AddrOfPinnedObject()))
using (Matrix<float> imagePointMatrix = new Matrix<float>(imagePoints.Length, 2, handle2.AddrOfPinnedObject()))
CvInvoke.cvFindExtrinsicCameraParams2(objectPointMatrix, imagePointMatrix, intrin.IntrinsicMatrix.Ptr, intrin.DistortionCoeffs.Ptr, p.RotationVector.Ptr, p.TranslationVector.Ptr, 0);
handle1.Free();
handle2.Free();
return p;
}
public void TestOctTree()
{
MCvPoint3D32f[] pts = new MCvPoint3D32f[]
{
new MCvPoint3D32f(1, 2, 3),
new MCvPoint3D32f(2, 3, 4),
new MCvPoint3D32f(4, 5, 6),
new MCvPoint3D32f(2, 2, 2)
};
using (Octree tree = new Octree(pts, 10, 10))
{
MCvPoint3D32f[] p = tree.GetPointsWithinSphere(new MCvPoint3D32f(0, 0, 0), 5);
int i = p.Length;
}
}
/// <summary>
/// Estimates transformation between the 2 cameras making a stereo pair. If we have a stereo camera, where the relative position and orientatation of the 2 cameras is fixed, and if we computed poses of an object relative to the fist camera and to the second camera, (R1, T1) and (R2, T2), respectively (that can be done with cvFindExtrinsicCameraParams2), obviously, those poses will relate to each other, i.e. given (R1, T1) it should be possible to compute (R2, T2) - we only need to know the position and orientation of the 2nd camera relative to the 1st camera. That's what the described function does. It computes (R, T) such that:
/// R2=R*R1,
/// T2=R*T1 + T
/// </summary>
/// <param name="objectPoints">The 3D location of the object points. The first index is the index of image, second index is the index of the point</param>
/// <param name="imagePoints1">The 2D image location of the points for camera 1. The first index is the index of the image, second index is the index of the point</param>
/// <param name="imagePoints2">The 2D image location of the points for camera 2. The first index is the index of the image, second index is the index of the point</param>
/// <param name="intrinsicParam1">The intrisinc parameters for camera 1, might contains some initial values. The values will be modified by this function.</param>
/// <param name="intrinsicParam2">The intrisinc parameters for camera 2, might contains some initial values. The values will be modified by this function.</param>
/// <param name="imageSize">Size of the image, used only to initialize intrinsic camera matrix</param>
/// <param name="flags">Different flags</param>
/// <param name="extrinsicParams">The extrinsic parameters which contains:
/// R - The rotation matrix between the 1st and the 2nd cameras' coordinate systems;
/// T - The translation vector between the cameras' coordinate systems. </param>
/// <param name="essentialMatrix">essential matrix</param>
/// <param name="termCrit"> Termination criteria for the iterative optimiziation algorithm </param>
/// <param name="foundamentalMatrix">fundamental matrix</param>
public static void StereoCalibrate(
MCvPoint3D32f[][] objectPoints,
PointF[][] imagePoints1,
PointF[][] imagePoints2,
IntrinsicCameraParameters intrinsicParam1,
IntrinsicCameraParameters intrinsicParam2,
Size imageSize,
CvEnum.CALIB_TYPE flags,
MCvTermCriteria termCrit,
out ExtrinsicCameraParameters extrinsicParams,
out Matrix<double> foundamentalMatrix,
out Matrix<double> essentialMatrix)
{
Debug.Assert(objectPoints.Length == imagePoints1.Length && objectPoints.Length == imagePoints2.Length, "The number of images for objects points should be equal to the number of images for image points");
#region get the matrix that represent the point counts
int[,] pointCounts = new int[objectPoints.Length, 1];
for (int i = 0; i < objectPoints.Length; i++)
{
Debug.Assert(objectPoints[i].Length == imagePoints1[i].Length && objectPoints[i].Length == imagePoints2[i].Length, String.Format("Number of 3D points and image points should be equal in the {0}th image", i));
pointCounts[i, 0] = objectPoints[i].Length;
}
#endregion
using (Matrix<float> objectPointMatrix = ToMatrix(objectPoints))
using (Matrix<float> imagePointMatrix1 = ToMatrix(imagePoints1))
using (Matrix<float> imagePointMatrix2 = ToMatrix(imagePoints2))
using (Matrix<int> pointCountsMatrix = new Matrix<int>(pointCounts))
{
extrinsicParams = new ExtrinsicCameraParameters();
essentialMatrix = new Matrix<double>(3, 3);
foundamentalMatrix = new Matrix<double>(3, 3);
CvInvoke.cvStereoCalibrate(
objectPointMatrix.Ptr,
imagePointMatrix1.Ptr,
imagePointMatrix2.Ptr,
pointCountsMatrix.Ptr,
intrinsicParam1.IntrinsicMatrix,
intrinsicParam1.DistortionCoeffs,
intrinsicParam2.IntrinsicMatrix,
intrinsicParam2.DistortionCoeffs,
imageSize,
extrinsicParams.RotationVector,
extrinsicParams.TranslationVector,
essentialMatrix.Ptr,
foundamentalMatrix.Ptr,
termCrit,
flags);
}
}
public void TestProjectPoints()
{
IntrinsicCameraParameters intrin = new IntrinsicCameraParameters();
intrin.IntrinsicMatrix.SetIdentity();
ExtrinsicCameraParameters extrin = new ExtrinsicCameraParameters();
MCvPoint3D32f point = new MCvPoint3D32f();
PointF[] points = CameraCalibration.ProjectPoints(new MCvPoint3D32f[] { point }, extrin, intrin);
}
//called when data for any output pin is requested
public void Evaluate(int SpreadMax)
{
if (FPinInDo[0])
{
int nPointsPerImage = FPinInObject.SliceCount;
bool useVVVVCoords = FPinInCoordSystem[0] == TCoordinateSystem.VVVV;
if (nPointsPerImage == 0)
{
FStatus[0] = "Insufficient points";
return;
}
int nImages = FPinInImage.SliceCount / nPointsPerImage;
MCvPoint3D32f[][] objectPoints = new MCvPoint3D32f[nImages][];
PointF[][] imagePoints = new PointF[nImages][];
Size imageSize = new Size( (int) FPinInSensorSize[0].x, (int) FPinInSensorSize[0].y);
CALIB_TYPE flags = new CALIB_TYPE();
IntrinsicCameraParameters intrinsicParam = new IntrinsicCameraParameters();
ExtrinsicCameraParameters[] extrinsicsPerView;
GetFlags(out flags);
if (flags.HasFlag(CALIB_TYPE.CV_CALIB_USE_INTRINSIC_GUESS))
{
if (FPinInIntrinsics[0] == null)
{
Matrix<double> mat = intrinsicParam.IntrinsicMatrix;
mat[0, 0] = FPinInSensorSize[0].x / 2.0d;
mat[1, 1] = FPinInSensorSize[0].y / 2.0d;
mat[0, 2] = FPinInSensorSize[0].x / 2.0d;
mat[1, 2] = FPinInSensorSize[0].y / 2.0d;
mat[2, 2] = 1;
}
else
{
intrinsicParam.DistortionCoeffs = FPinInIntrinsics[0].intrinsics.DistortionCoeffs.Clone();
intrinsicParam.IntrinsicMatrix = FPinInIntrinsics[0].intrinsics.IntrinsicMatrix.Clone();
}
}
imagePoints = MatrixUtils.ImagePoints(FPinInImage, nPointsPerImage);
for (int i=0; i<nImages; i++)
{
objectPoints[i] = MatrixUtils.ObjectPoints(FPinInObject, useVVVVCoords);
}
try
{
FPinOutError[0] = CameraCalibration.CalibrateCamera(objectPoints, imagePoints, imageSize, intrinsicParam, flags, out extrinsicsPerView);
Intrinsics intrinsics = new Intrinsics(intrinsicParam, imageSize);
FPinOutIntrinsics[0] = intrinsics;
if (useVVVVCoords)
FPinOutProjection[0] = intrinsics.Matrix;
else
FPinOutProjection[0] = intrinsics.Matrix;
FPinOutExtrinsics.SliceCount = nImages;
FPinOutView.SliceCount = nImages;
for (int i = 0; i < nImages; i++)
{
Extrinsics extrinsics = new Extrinsics(extrinsicsPerView[i]);
FPinOutExtrinsics[i] = extrinsics;
if (useVVVVCoords)
FPinOutView[i] = MatrixUtils.ConvertToVVVV(extrinsics.Matrix);
else
FPinOutView[i] = extrinsics.Matrix;
}
FPinOutSuccess[0] = true;
FStatus[0] = "OK";
}
catch (Exception e) {
FPinOutSuccess[0] = false;
FStatus[0] = e.Message;
}
}
}
public void TestCrossProduct()
{
MCvPoint3D32f p1 = new MCvPoint3D32f(1.0f, 0.0f, 0.0f);
MCvPoint3D32f p2 = new MCvPoint3D32f(0.0f, 1.0f, 0.0f);
MCvPoint3D32f p3 = p1.CrossProduct(p2);
Assert.IsTrue(new MCvPoint3D32f(0.0f, 0.0f, 1.0f).Equals(p3));
}
