/// <summary>
/// CvPoint3D32fをCameraSpacePointに変換します
/// </summary>
/// <param name="point"></param>
/// <returns></returns>
public static CameraSpacePoint ToCameraSpacePoint(this CvPoint3D32f point)
{
return(new CameraSpacePoint()
{
X = point.X, Y = point.Y, Z = point.Z
});
}
/// <summary>
/// cvCreatePOSITObjectで初期化
/// </summary>
/// <param name="points">3次元オブジェクトモデル上の点データの配列</param>
#else
/// <summary>
/// Constructor (cvCreatePOSITObject)
/// </summary>
/// <param name="points">Points of the 3D object model. </param>
#endif
public CvPOSITObject(CvPoint3D32f[] points)
{
if (points == null)
throw new ArgumentNullException("points");
ptr = NativeMethods.cvCreatePOSITObject(points, points.Length);
if (ptr == IntPtr.Zero)
throw new OpenCvSharpException();
}
/// <summary>
/// cvCreatePOSITObjectで初期化
/// </summary>
/// <param name="points">3次元オブジェクトモデル上の点データの配列</param>
#else
/// <summary>
/// Constructor (cvCreatePOSITObject)
/// </summary>
/// <param name="points">Points of the 3D object model. </param>
#endif
public CvPOSITObject(CvPoint3D32f[] points)
{
if (points == null)
{
throw new ArgumentNullException("points");
}
this.ptr = CvInvoke.cvCreatePOSITObject(points, points.Length);
if (this.ptr == IntPtr.Zero)
{
throw new OpenCvSharpException();
}
}
private CvMat PutObjectPointsIntoCVMat(List <Vector3> _objectPositions, int pointsCount, int ImageNum)
{
CvPoint3D32f[,] objects = new CvPoint3D32f[ImageNum, pointsCount];
for (int i = 0; i < pointsCount; i++)
{
objects[0, i] = new CvPoint3D32f
{
X = _objectPositions[i].x,
Y = _objectPositions[i].y,
Z = _objectPositions[i].z * -1
};
}
return(new CvMat(pointsCount, 3, MatrixType.F32C1, objects));
}
/// <summary>
/// Генератор координат углов шахматной доски с заданным размером
/// </summary>
/// <returns>Сгенерированные координаты углов</returns>
private CvMat generateCorners()
{
int rowNum = CornersPattern.Height;
int colNum = CornersPattern.Width;
CvPoint3D32f[,] corners = new CvPoint3D32f[rowNum, colNum];
for (int i = 0; i < rowNum; i++)
{
for (int j = 0; j < colNum; j++)
{
corners[i, j] = new CvPoint3D32f(i * CellSize, j * CellSize, 0);
}
}
return(new CvMat(rowNum * colNum, 3, MatrixType.F32C1, corners));
}
/// <summary>
/// 足の長さを統計情報をもとに正規化する
/// </summary>
/// <param name="body"></param>
private Dictionary <JointType, Joint> NormalizeLegJoints(Dictionary <JointType, Joint> joints, Dictionary <Bone, BoneStatistics> boneStatistics)
{
List <Tuple <JointType, JointType> > legBones = Utility.GetLegBones();
foreach (Bone bone in legBones)
{
if (joints.ContainsKey(bone.Item1) && joints.ContainsKey(bone.Item2))
{
Joint joint1 = joints[bone.Item1];
Joint joint2 = joints[bone.Item2];
// 1の骨は動かさない. SpineBaseから順番に修正されることは保証されている.
double medianLength = Math.Sqrt(boneStatistics[bone].medianLengthSq);
CvPoint3D64f normalizedVector = CvEx.Normalize(joint1.Position.ToCvPoint3D() - joint2.Position.ToCvPoint3D());
CvPoint3D32f expandedVector = (CvPoint3D32f)(normalizedVector * medianLength);
joint2.Position = (joint1.Position.ToCvPoint3D() + expandedVector).ToCameraSpacePoint();
joints[bone.Item2] = joint2;
}
}
return(joints);
}
public static CameraCalibrationData CalibrateLens(List<Bitmap> bitmaps, int calibrationTakes)
{
int ImageNum = calibrationTakes;
const int PatRow = 7;
const int PatCol = 10;
const int PatSize = PatRow * PatCol;
int AllPoints = ImageNum * PatSize;
const float ChessSize = 24.0f;
// Convert bitmaps into Ipl Images
IplImage[] srcImg = new IplImage[bitmaps.Count];
for (int i = 0; i < bitmaps.Count; i++)
srcImg[i] = OpenCVUtil.IplImageFromBitmap(bitmaps[i]);
CvPoint3D32f[, ,] objects = new CvPoint3D32f[ImageNum, PatRow, PatCol];
for (int i = 0; i < ImageNum; i++)
{
for (int j = 0; j < PatRow; j++)
{
for (int k = 0; k < PatCol; k++)
{
objects[i, j, k] = new CvPoint3D32f
{
X = j * ChessSize,
Y = k * ChessSize,
Z = 0.0f
};
}
}
}
CvMat objectPoints = new CvMat(AllPoints, 3, MatrixType.F32C1, objects);
CvSize patternSize = new CvSize(PatCol, PatRow);
int foundNum = 0;
List<CvPoint2D32f> allCorners = new List<CvPoint2D32f>(AllPoints);
int[] pointCountsValue = new int[ImageNum];
using (CvWindow window = new CvWindow("Calibration", WindowMode.AutoSize))
{
for (int i = 0; i < ImageNum; i++)
{
CvPoint2D32f[] corners;
bool found = Cv.FindChessboardCorners(srcImg[i], patternSize, out corners);
Debug.Print("{0:D2}...", i);
if (found)
{
Debug.Print("ok");
foundNum++;
}
else
{
Debug.Print("fail");
}
using (IplImage srcGray = new IplImage(srcImg[i].Size, BitDepth.U8, 1))
{
Cv.CvtColor(srcImg[i], srcGray, ColorConversion.BgrToGray);
Cv.FindCornerSubPix(srcGray, corners, corners.Length, new CvSize(3, 3), new CvSize(-1, -1), new CvTermCriteria(20, 0.03));
Cv.DrawChessboardCorners(srcImg[i], patternSize, corners, found);
pointCountsValue[i] = corners.Length;
window.ShowImage(srcImg[i]);
//Cv.WaitKey(0);
}
allCorners.AddRange(corners);
}
if (foundNum != ImageNum)
{
Debug.Assert(false);
}
}
CvMat imagePoints = new CvMat(AllPoints, 1, MatrixType.F32C2, allCorners.ToArray());
CvMat pointCounts = new CvMat(ImageNum, 1, MatrixType.S32C1, pointCountsValue);
CvMat intrinsic = new CvMat(3, 3, MatrixType.F64C1);
CvMat distortion = new CvMat(1, 4, MatrixType.F64C1);
CvMat rotation = new CvMat(ImageNum, 3, MatrixType.F64C1);
CvMat translation = new CvMat(ImageNum, 3, MatrixType.F64C1);
Cv.CalibrateCamera2(objectPoints, imagePoints, pointCounts, srcImg[0].Size, intrinsic, distortion, rotation, translation, CalibrationFlag.Default);
CvMat subImagePoints, subObjectPoints;
Cv.GetRows(imagePoints, out subImagePoints, 0, PatSize);
Cv.GetRows(objectPoints, out subObjectPoints, 0, PatSize);
CvMat rotation_ = new CvMat(1, 3, MatrixType.F32C1);
CvMat translation_ = new CvMat(1, 3, MatrixType.F32C1);
Cv.FindExtrinsicCameraParams2(subObjectPoints, subImagePoints, intrinsic, distortion, rotation_, translation_, false);
//Cv.FindExtrinsicCameraParams2_(subObjectPoints, subImagePoints, intrinsic, distortion, rotation_, translation_, false);
// Free IplImages
foreach (IplImage img in srcImg)
img.Dispose();
// Construct and return camera calibration data
CameraCalibrationData camCal = new CameraCalibrationData();
camCal.IsValid = foundNum > (ImageNum / 2); // Consider valid if at least half were successes
camCal.Intrinsic = intrinsic;
camCal.Translation = translation_;
camCal.Rotation = rotation_;
camCal.Distortion = distortion;
return camCal;
}
private void buttonScalingScore_Click(object sender, RoutedEventArgs e)
{
int cols, rows;
double horizLength, vertLength;
if (!parseChessboardParameters(out cols, out rows, out horizLength, out vertLength))
{
return;
}
// 以下改造
MotionDataHandler handler;
string path;
if (openMotionData(out handler, out path))
{
CvMat displayMat1 = null;
CvMat displayMat3 = null;
CvMat displayMat4 = null;
CvMat gray = null;
int length = handler.FrameCount;
if (length == 0)
{
return;
}
CvSize boardSize = new CvSize(cols, rows);
CvSize imageSize = new CvSize();
List <Tuple <double, double> > pairs = new List <Tuple <double, double> >();
CvPoint2D32f[] lastCorners = null;
IEnumerable <CvMat> colorImages, depthImages;
Utility.LoadImages(handler.GetColorImagePaths(), out colorImages);
Utility.LoadImages(handler.GetDepthImagePaths(), out depthImages);
var images = colorImages.Zip(depthImages, (first, second) => Tuple.Create(first, second));
foreach (Tuple <CvMat, CvMat> imagePair in images)
{
CvMat imageMat = imagePair.Item1;
CvMat depthMat = imagePair.Item2;
if (displayMat4 == null)
{
displayMat4 = CvEx.InitCvMat(imageMat);
}
imageSize = new CvSize(imageMat.Cols, imageMat.Rows);
CvPoint2D32f[] corners;
int count;
CvEx.InitCvMat(ref gray, imageMat, MatrixType.U8C1);
imageMat.CvtColor(gray, ColorConversion.RgbToGray);
if (gray.FindChessboardCorners(boardSize, out corners, out count, ChessboardFlag.AdaptiveThresh))
{
CvEx.CloneCvMat(ref displayMat1, imageMat);
CvTermCriteria criteria = new CvTermCriteria(50, 0.01);
gray.FindCornerSubPix(corners, count, new CvSize(3, 3), new CvSize(-1, -1), criteria);
CvPoint3D32f?[] cornerPoints = new CvPoint3D32f?[corners.Length];
for (int j = 0; j < corners.Length; j++)
{
CvPoint2D32f corner = corners[j];
double? value = CalcEx.BilateralFilterDepthMatSinglePixel(corner, depthMat, 100, 4, 9);
if (value.HasValue)
{
cornerPoints[j] = new CvPoint3D32f(corner.X, corner.Y, value.Value);
}
}
for (int x = 0; x < cols; x++)
{
for (int y = 0; y < rows; y++)
{
if (!cornerPoints[x + y * cols].HasValue)
{
continue;
}
CvPoint3D32f point1 = cornerPoints[x + y * cols].Value;
CvPoint3D64f undistortPoint1 = this.UndistortionData.GetRealFromScreenPos(point1, imageSize);
foreach (var offset in new[] { new { X = 1, Y = 0, D = horizLength }, new { X = 0, Y = 1, D = vertLength } })
{
int dx = x + offset.X;
int dy = y + offset.Y;
if (dx >= cols || dy >= rows)
{
continue;
}
if (!cornerPoints[dx + dy * cols].HasValue)
{
continue;
}
CvPoint3D32f point2 = cornerPoints[dx + dy * cols].Value;
CvPoint3D64f undistortPoint2 = this.UndistortionData.GetRealFromScreenPos(point2, imageSize);
double distance = Math.Sqrt(CvEx.GetDistanceSq(undistortPoint1, undistortPoint2));
double scale = distance / offset.D;
CvColor color = CalcEx.HSVtoRGB(Math.Max(0, Math.Min(300, scale * 600 - 450)), scale, 2 - scale);
displayMat4.DrawLine((int)point1.X, (int)point1.Y, (int)point2.X, (int)point2.Y, new CvScalar(color.R, color.G, color.B), 1, LineType.AntiAlias);
pairs.Add(new Tuple <double, double>(distance, offset.D));
}
}
}
CvEx.DrawChessboardCornerFrame(displayMat1, boardSize, corners, new CvScalar(64, 128, 64));
displayMat1.DrawChessboardCorners(boardSize, corners, true);
lastCorners = corners;
putImage(displayMat1, PixelFormats.Rgb24);
}
else
{
CvEx.CloneCvMat(ref displayMat3, imageMat);
putImage(displayMat3, PixelFormats.Rgb24);
}
}
CvMat displayMat2 = CvEx.InitCvMat(displayMat1);
displayMat1.Undistort2(displayMat2, this.UndistortionData.CameraStruct.CreateCvMat(), this.UndistortionData.DistortStruct.CreateCvMat(true));
if (lastCorners != null)
{
drawUndistortedCornerFrame(displayMat2, lastCorners, boardSize);
}
displayMat2.PutText(string.Format("Min: {0}", pairs.Min(x => x.Item1 / x.Item2)), new CvPoint(20, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Max: {0}", pairs.Max(x => x.Item1 / x.Item2)), new CvPoint(20, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Avg: {0}", pairs.Average(x => x.Item1 / x.Item2)), new CvPoint(20, 60), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Med: {0}", CalcEx.GetMedian(pairs.Select(x => x.Item1 / x.Item2).ToList())), new CvPoint(20, 80), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
putImage(displayMat4, PixelFormats.Rgb24);
displayLabels();
}
}
/// <summary>
/// Генератор координат углов шахматной доски с заданным размером
/// </summary>
/// <returns>Сгенерированные координаты углов</returns>
private CvMat generateCorners()
{
int rowNum = CornersPattern.Height;
int colNum = CornersPattern.Width;
CvPoint3D32f[,] corners = new CvPoint3D32f[rowNum, colNum];
for (int i = 0; i < rowNum; i++) for (int j = 0; j < colNum; j++)
{
corners[i, j] = new CvPoint3D32f(i * CellSize, j * CellSize, 0);
}
return new CvMat(rowNum * colNum, 3, MatrixType.F32C1, corners);
}
public CalibrateCamera()
{
const int ImageNum = 3;
const int PatRow = 7;
const int PatCol = 10;
const int PatSize = PatRow * PatCol;
const int AllPoints = ImageNum * PatSize;
const float ChessSize = 24.0f;
IplImage[] srcImg = new IplImage[ImageNum];
for (int i = 0; i < ImageNum; i++)
{
srcImg[i] = new IplImage(string.Format(FilePath.Image.Calibration, i), LoadMode.Color);
}
CvPoint3D32f[,,] objects = new CvPoint3D32f[ImageNum, PatRow, PatCol];
for (int i = 0; i < ImageNum; i++)
{
for (int j = 0; j < PatRow; j++)
{
for (int k = 0; k < PatCol; k++)
{
objects[i, j, k] = new CvPoint3D32f
{
X = j * ChessSize,
Y = k * ChessSize,
Z = 0.0f
};
}
}
}
CvMat objectPoints = new CvMat(AllPoints, 3, MatrixType.F32C1, objects);
CvSize patternSize = new CvSize(PatCol, PatRow);
int foundNum = 0;
List<CvPoint2D32f> allCorners = new List<CvPoint2D32f>(AllPoints);
int[] pointCountsValue = new int[ImageNum];
using (CvWindow window = new CvWindow("Calibration", WindowMode.AutoSize))
{
for (int i = 0; i < ImageNum; i++)
{
CvPoint2D32f[] corners;
bool found = Cv.FindChessboardCorners(srcImg[i], patternSize, out corners);
Debug.Print("{0:D2}...", i);
if (found)
{
Debug.Print("ok");
foundNum++;
}
else
{
Debug.Print("fail");
}
using (IplImage srcGray = new IplImage(srcImg[i].Size, BitDepth.U8, 1))
{
Cv.CvtColor(srcImg[i], srcGray, ColorConversion.BgrToGray);
Cv.FindCornerSubPix(srcGray, corners, corners.Length, new CvSize(3, 3), new CvSize(-1, -1), new CvTermCriteria(20, 0.03));
Cv.DrawChessboardCorners(srcImg[i], patternSize, corners, found);
pointCountsValue[i] = corners.Length;
window.ShowImage(srcImg[i]);
Cv.WaitKey(0);
}
allCorners.AddRange(corners);
}
if (foundNum != ImageNum)
{
Debug.Assert(false);
}
}
CvMat imagePoints = new CvMat(AllPoints, 1, MatrixType.F32C2, allCorners.ToArray());
CvMat pointCounts = new CvMat(ImageNum, 1, MatrixType.S32C1, pointCountsValue);
CvMat intrinsic = new CvMat(3, 3, MatrixType.F64C1);
CvMat distortion = new CvMat(1, 4, MatrixType.F64C1);
CvMat rotation = new CvMat(ImageNum, 3, MatrixType.F64C1);
CvMat translation = new CvMat(ImageNum, 3, MatrixType.F64C1);
Cv.CalibrateCamera2(objectPoints, imagePoints, pointCounts, srcImg[0].Size, intrinsic, distortion, rotation, translation, CalibrationFlag.Default);
CvMat subImagePoints, subObjectPoints;
Cv.GetRows(imagePoints, out subImagePoints, 0, PatSize);
Cv.GetRows(objectPoints, out subObjectPoints, 0, PatSize);
CvMat rotation_ = new CvMat(1, 3, MatrixType.F32C1);
CvMat translation_ = new CvMat(1, 3, MatrixType.F32C1);
Cv.FindExtrinsicCameraParams2(subObjectPoints, subImagePoints, intrinsic, distortion, rotation_, translation_, false);
//Cv.FindExtrinsicCameraParams2_(subObjectPoints, subImagePoints, intrinsic, distortion, rotation_, translation_, false);
using (var fs = new CvFileStorage("camera.xml", null, OpenCvSharp.FileStorageMode.Write))
{
fs.Write("intrinsic", intrinsic);
fs.Write("rotation", rotation_);
fs.Write("translation", translation_);
fs.Write("distortion", distortion);
}
foreach (IplImage img in srcImg)
{
img.Dispose();
}
// 書き込んだファイルを表示
Console.WriteLine(File.ReadAllText("camera.xml"));
Console.Read();
}
public CalibrateStereoCamera()
{
// target filenames
string[] pair = new string[] { "Data/Image/Calibration/left{0:D2}.jpg", "Data/Image/Calibration/right{0:D2}.jpg" };
string[][] fileNames = new string[ImageNum][];
for (int i = 0; i < ImageNum; i++)
{
fileNames[i] = new string[2] { string.Format(pair[0], i + 1), string.Format(pair[1], i + 1) };
}
// FindChessboardCorners
CvPoint2D32f[] imagePointsLeft, imagePointsRight;
int[] pointCountLeft, pointCountRight;
int[] goodImagelist;
FindChessboardCorners(fileNames, out imagePointsLeft, out imagePointsRight, out pointCountLeft, out pointCountRight, out goodImagelist);
int nImages = goodImagelist.Length;
// StereoCalibrate
CvPoint3D32f[, ,] objects = new CvPoint3D32f[ImageNum, BoardSize.Height, BoardSize.Width];
for (int i = 0; i < ImageNum; i++)
for (int j = 0; j < BoardSize.Height; j++)
for (int k = 0; k < BoardSize.Width; k++)
objects[i, j, k] = new CvPoint3D32f(j * SquareSize, k * SquareSize, 0.0f);
CvMat objectPoints = new CvMat(AllPoints, 3, MatrixType.F32C1, objects);
CvMat imagePoints1 = new CvMat(AllPoints, 1, MatrixType.F32C2, imagePointsLeft);
CvMat imagePoints2 = new CvMat(AllPoints, 1, MatrixType.F32C2, imagePointsRight);
CvMat pointCount1 = new CvMat(nImages, 1, MatrixType.S32C1, pointCountLeft);
CvMat pointCount2 = new CvMat(nImages, 1, MatrixType.S32C1, pointCountRight);
CvMat cameraMatrix1 = CvMat.Identity(3, 3, MatrixType.F64C1);
CvMat cameraMatrix2 = CvMat.Identity(3, 3, MatrixType.F64C1);
CvMat distCoeffs1 = new CvMat(1, 4, MatrixType.F64C1);
CvMat distCoeffs2 = new CvMat(1, 4, MatrixType.F64C1);
CvMat R = new CvMat(3, 3, MatrixType.F64C1);
CvMat T = new CvMat(3, 1, MatrixType.F64C1);
Cv.StereoCalibrate(objectPoints, imagePoints1, imagePoints2, pointCount1,
cameraMatrix1, distCoeffs1,
cameraMatrix2, distCoeffs2,
new CvSize(640, 480), R, T, null, null,
new CvTermCriteria(100, 1e-5),
CalibrationFlag.FixAspectRatio | CalibrationFlag.ZeroTangentDist | CalibrationFlag.SameFocalLength |
CalibrationFlag.RationalModel | CalibrationFlag.FixK3 | CalibrationFlag.FixK4 | CalibrationFlag.FixK5);
// Rectify
CvMat R1 = new CvMat(3, 3, MatrixType.F64C1);
CvMat R2 = new CvMat(3, 3, MatrixType.F64C1);
CvMat P1 = new CvMat(3, 4, MatrixType.F64C1);
CvMat P2 = new CvMat(3, 4, MatrixType.F64C1);
CvMat Q = new CvMat(4, 4, MatrixType.F64C1);
Cv.StereoRectify(cameraMatrix1, cameraMatrix2, distCoeffs1, distCoeffs2,
new CvSize(640, 480), R, T, R1, R2, P1, P2, Q,
StereoRectificationFlag.ZeroDisparity, 1, new CvSize(640, 480));
using (CvMemStorage mem = new CvMemStorage())
using(CvFileStorage fs = new CvFileStorage("extrinsic.yml", mem, FileStorageMode.Write))
{
fs.Write("R", R);
fs.Write("T", T);
fs.Write("R1", R1);
fs.Write("R2", R2);
fs.Write("P1", P1);
fs.Write("P1", P1);
fs.Write("Q", Q);
}
Process.Start("notepad", "extrinsic.yml");
Console.Read();
}
private void buttonTest0_Click(object sender, RoutedEventArgs e)
{
int cols, rows;
double horizLength, vertLength;
if (!parseChessboardParameters(out cols, out rows, out horizLength, out vertLength))
{
return;
}
// 以下修正
MotionDataHandler handler;
string path;
if (openMotionData(out handler, out path))
{
CvMat displayMat1 = null;
CvMat displayMat3 = null;
CvMat displayMat4 = null;
CvMat gray = null;
int length = handler.FrameCount;
if (length == 0)
{
return;
}
CvSize boardSize = new CvSize(cols, rows);
CvSize imageSize = new CvSize();
double minVarDistance2d = double.MaxValue;
IEnumerable <CvMat> colorImages, depthImages;
Utility.LoadImages(handler.GetColorImagePaths(), out colorImages);
Utility.LoadImages(handler.GetDepthImagePaths(), out depthImages);
var images = colorImages.Zip(depthImages, (first, second) => Tuple.Create(first, second));
foreach (Tuple <CvMat, CvMat> imagePair in images)
{
CvMat imageMat = imagePair.Item1;
CvMat depthMat = imagePair.Item2;
if (displayMat4 == null)
{
displayMat4 = CvEx.InitCvMat(imageMat);
}
imageSize = new CvSize(imageMat.Cols, imageMat.Rows);
CvSize depthUserSize = new CvSize(depthMat.Cols, depthMat.Rows);
CvPoint2D32f[] corners;
int count;
CvEx.InitCvMat(ref gray, imageMat, MatrixType.U8C1);
imageMat.CvtColor(gray, ColorConversion.RgbToGray);
if (gray.FindChessboardCorners(boardSize, out corners, out count, ChessboardFlag.AdaptiveThresh))
{
CvEx.CloneCvMat(ref displayMat1, imageMat);
CvTermCriteria criteria = new CvTermCriteria(50, 0.01);
gray.FindCornerSubPix(corners, count, new CvSize(3, 3), new CvSize(-1, -1), criteria);
CvPoint3D32f?[] cornerPoints = new CvPoint3D32f?[corners.Length];
for (int j = 0; j < corners.Length; j++)
{
CvPoint2D32f corner = new CvPoint2D32f(corners[j].X - 10, corners[j].Y - 10);
double? value = CvEx.Get2DSubPixel(depthMat, corner, 0);
if (value.HasValue)
{
double depth = UndistortionData.UndistortDepth(corner.X, corner.Y, value.Value, depthUserSize);
cornerPoints[j] = new CvPoint3D32f(corner.X, corner.Y, depth);
}
}
List <double> distance2dList = new List <double>();
for (int x = 0; x < cols; x++)
{
for (int y = 0; y < rows; y++)
{
if (!cornerPoints[x + y * cols].HasValue)
{
continue;
}
int nextX = x + 1;
if (nextX < cols)
{
if (!cornerPoints[nextX + y * cols].HasValue)
{
continue;
}
CvPoint3D32f point = cornerPoints[x + y * cols].Value;
CvPoint3D32f nextPoint = cornerPoints[nextX + y * cols].Value;
distance2dList.Add(Math.Sqrt(Math.Pow(point.X - nextPoint.X, 2) + Math.Pow(point.Y - nextPoint.Y, 2)));
}
int nextY = y + 1;
if (nextY < rows)
{
if (!cornerPoints[x + nextY * cols].HasValue)
{
continue;
}
CvPoint3D32f point = cornerPoints[x + y * cols].Value;
CvPoint3D32f nextPoint = cornerPoints[x + nextY * cols].Value;
distance2dList.Add(Math.Sqrt(Math.Pow(point.X - nextPoint.X, 2) + Math.Pow(point.Y - nextPoint.Y, 2)));
}
}
}
if (distance2dList.Count >= 2)
{
double stdevDistance2d = CalcEx.GetStdDev(distance2dList);
displayMat1.PutText(string.Format("{0:0.00}/{1:0.00}", stdevDistance2d, minVarDistance2d), new CvPoint(0, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 0));
double avgDepth = cornerPoints.Where(p => p.HasValue).Select(p => p.Value.Z).Average();
for (int x = 0; x < cols; x++)
{
for (int y = 0; y < rows; y++)
{
if (!cornerPoints[x + y * cols].HasValue)
{
continue;
}
CvPoint3D32f point = cornerPoints[x + y * cols].Value;
displayMat1.PutText((point.Z - avgDepth).ToString("0.00"), new CvPoint((int)point.X, (int)point.Y), new CvFont(FontFace.HersheyPlain, 0.6, 0.6), new CvScalar(255, 0, 0));
displayMat1.PutText(((point.Z - avgDepth) / avgDepth * 100).ToString("0.000"), new CvPoint((int)point.X, (int)point.Y + 12), new CvFont(FontFace.HersheyPlain, 0.6, 0.6), new CvScalar(0, 255, 255));
}
}
//displayMat1.DrawChessboardCorners(boardSize, corners, true);
if (stdevDistance2d < minVarDistance2d)
{
minVarDistance2d = stdevDistance2d;
CvEx.CloneCvMat(ref displayMat4, displayMat1);
}
//System.Threading.Thread.Sleep(500);
}
putImage(displayMat1, PixelFormats.Rgb24);
}
else
{
CvEx.CloneCvMat(ref displayMat3, imageMat);
putImage(displayMat3, PixelFormats.Rgb24);
}
}
putImage(displayMat4, PixelFormats.Rgb24);
displayLabels();
}
}
public static extern void cvCalcImageHomography([MarshalAs(UnmanagedType.LPArray)] float[] line, ref CvPoint3D32f center,
[MarshalAs(UnmanagedType.LPArray)] float[,] intrinsic, [Out] [MarshalAs(UnmanagedType.LPArray)] float[,] homography);
private void buttonCalibrateScaleOffset_Click(object sender, RoutedEventArgs e)
{
int cols, rows;
double horizLength, vertLength;
if (!parseChessboardParameters(out cols, out rows, out horizLength, out vertLength))
{
return;
}
// 以下改造
MotionDataHandler handler;
string path;
if (openMotionData(out handler, out path))
{
CvMat displayMat1 = null;
CvMat displayMat3 = null;
CvMat gray = null;
if (ProgressData.DoAction(progress =>
{
int length = handler.FrameCount;
if (length == 0)
{
return;
}
progress.InitProgress("Find Chessboard...", length * 2);
CvSize boardSize = new CvSize(cols, rows);
List <CvPoint3D32f?[]> list = new List <CvPoint3D32f?[]>();
CvSize imageSize = new CvSize();
CvPoint2D32f[] lastCorners = null;
IEnumerable <CvMat> colorImages, depthImages;
Utility.LoadImages(handler.GetColorImagePaths(), out colorImages);
Utility.LoadImages(handler.GetDepthImagePaths(), out depthImages);
var images = colorImages.Zip(depthImages, (first, second) => Tuple.Create(first, second));
foreach (Tuple <CvMat, CvMat> imagePair in images)
{
progress.CurrentValue++;
CvMat imageMat = imagePair.Item1;
CvMat depthMat = imagePair.Item2;
imageSize = new CvSize(imageMat.Cols, imageMat.Rows);
CvPoint2D32f[] corners;
int count;
CvEx.InitCvMat(ref gray, imageMat, MatrixType.U8C1);
imageMat.CvtColor(gray, ColorConversion.RgbToGray);
if (gray.FindChessboardCorners(boardSize, out corners, out count, ChessboardFlag.AdaptiveThresh))
{
CvEx.CloneCvMat(ref displayMat1, imageMat);
CvTermCriteria criteria = new CvTermCriteria(50, 0.01);
gray.FindCornerSubPix(corners, count, new CvSize(3, 3), new CvSize(-1, -1), criteria);
CvPoint3D32f?[] cornerPoints = new CvPoint3D32f?[corners.Length];
for (int j = 0; j < corners.Length; j++)
{
CvPoint2D32f corner = corners[j];
double?value = CalcEx.BilateralFilterDepthMatSinglePixel(corner, depthMat, 100, 4, 9);
if (value.HasValue)
{
cornerPoints[j] = new CvPoint3D32f(corner.X, corner.Y, value.Value);
}
}
list.Add(cornerPoints);
CvEx.DrawChessboardCornerFrame(displayMat1, boardSize, corners, new CvScalar(64, 128, 64));
displayMat1.DrawChessboardCorners(boardSize, corners, true);
lastCorners = corners;
//putImage(displayMat1, PixelFormats.Bgr24);
}
else
{
CvEx.CloneCvMat(ref displayMat3, imageMat);
//putImage(displayMat3, PixelFormats.Bgr24);
}
}
progress.SetProgress("Scale Offset Calibrating...", length);
this.UndistortionData.CalibrateRealScaleAndOffset(list, cols, rows, horizLength, vertLength, imageSize);
CvMat displayMat2 = CvEx.InitCvMat(displayMat1);
displayMat1.Undistort2(displayMat2, this.UndistortionData.CameraStruct.CreateCvMat(), this.UndistortionData.DistortStruct.CreateCvMat(true));
if (lastCorners != null)
{
drawUndistortedCornerFrame(displayMat2, lastCorners, boardSize);
}
displayMat2.PutText(string.Format("XScale: {0}", this.UndistortionData.XScale), new CvPoint(20, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("YScale: {0}", this.UndistortionData.YScale), new CvPoint(20, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Zoffset: {0}", this.UndistortionData.ZOffset), new CvPoint(20, 60), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
putImage(displayMat2, PixelFormats.Bgr24);
}, "Calibrate Scale Offset", true))
{
displayLabels();
}
}
}
public static CameraCalibrationData CalibrateLens(List <Bitmap> bitmaps, int calibrationTakes)
{
int ImageNum = calibrationTakes;
const int PatRow = 7;
const int PatCol = 10;
const int PatSize = PatRow * PatCol;
int AllPoints = ImageNum * PatSize;
const float ChessSize = 24.0f;
// Convert bitmaps into Ipl Images
IplImage[] srcImg = new IplImage[bitmaps.Count];
for (int i = 0; i < bitmaps.Count; i++)
{
srcImg[i] = OpenCVUtil.IplImageFromBitmap(bitmaps[i]);
}
CvPoint3D32f[, ,] objects = new CvPoint3D32f[ImageNum, PatRow, PatCol];
for (int i = 0; i < ImageNum; i++)
{
for (int j = 0; j < PatRow; j++)
{
for (int k = 0; k < PatCol; k++)
{
objects[i, j, k] = new CvPoint3D32f
{
X = j * ChessSize,
Y = k * ChessSize,
Z = 0.0f
};
}
}
}
CvMat objectPoints = new CvMat(AllPoints, 3, MatrixType.F32C1, objects);
CvSize patternSize = new CvSize(PatCol, PatRow);
int foundNum = 0;
List <CvPoint2D32f> allCorners = new List <CvPoint2D32f>(AllPoints);
int[] pointCountsValue = new int[ImageNum];
using (CvWindow window = new CvWindow("Calibration", WindowMode.AutoSize))
{
for (int i = 0; i < ImageNum; i++)
{
CvPoint2D32f[] corners;
bool found = Cv.FindChessboardCorners(srcImg[i], patternSize, out corners);
Debug.Print("{0:D2}...", i);
if (found)
{
Debug.Print("ok");
foundNum++;
}
else
{
Debug.Print("fail");
}
using (IplImage srcGray = new IplImage(srcImg[i].Size, BitDepth.U8, 1))
{
Cv.CvtColor(srcImg[i], srcGray, ColorConversion.BgrToGray);
Cv.FindCornerSubPix(srcGray, corners, corners.Length, new CvSize(3, 3), new CvSize(-1, -1), new CvTermCriteria(20, 0.03));
Cv.DrawChessboardCorners(srcImg[i], patternSize, corners, found);
pointCountsValue[i] = corners.Length;
window.ShowImage(srcImg[i]);
//Cv.WaitKey(0);
}
allCorners.AddRange(corners);
}
if (foundNum != ImageNum)
{
Debug.Assert(false);
}
}
CvMat imagePoints = new CvMat(AllPoints, 1, MatrixType.F32C2, allCorners.ToArray());
CvMat pointCounts = new CvMat(ImageNum, 1, MatrixType.S32C1, pointCountsValue);
CvMat intrinsic = new CvMat(3, 3, MatrixType.F64C1);
CvMat distortion = new CvMat(1, 4, MatrixType.F64C1);
CvMat rotation = new CvMat(ImageNum, 3, MatrixType.F64C1);
CvMat translation = new CvMat(ImageNum, 3, MatrixType.F64C1);
Cv.CalibrateCamera2(objectPoints, imagePoints, pointCounts, srcImg[0].Size, intrinsic, distortion, rotation, translation, CalibrationFlag.Default);
CvMat subImagePoints, subObjectPoints;
Cv.GetRows(imagePoints, out subImagePoints, 0, PatSize);
Cv.GetRows(objectPoints, out subObjectPoints, 0, PatSize);
CvMat rotation_ = new CvMat(1, 3, MatrixType.F32C1);
CvMat translation_ = new CvMat(1, 3, MatrixType.F32C1);
Cv.FindExtrinsicCameraParams2(subObjectPoints, subImagePoints, intrinsic, distortion, rotation_, translation_, false);
//Cv.FindExtrinsicCameraParams2_(subObjectPoints, subImagePoints, intrinsic, distortion, rotation_, translation_, false);
// Free IplImages
foreach (IplImage img in srcImg)
{
img.Dispose();
}
// Construct and return camera calibration data
CameraCalibrationData camCal = new CameraCalibrationData();
camCal.IsValid = foundNum > (ImageNum / 2); // Consider valid if at least half were successes
camCal.Intrinsic = intrinsic;
camCal.Translation = translation_;
camCal.Rotation = rotation_;
camCal.Distortion = distortion;
return(camCal);
}
private CvMat PutObjectPointsIntoCVMat(List<Vector3> _objectPositions, int pointsCount, int ImageNum)
{
CvPoint3D32f[,] objects = new CvPoint3D32f[ImageNum, pointsCount];
for (int i = 0; i < pointsCount; i++)
{
objects[0, i] = new CvPoint3D32f
{
X = _objectPositions[i].x,
Y = _objectPositions[i].y,
Z = _objectPositions[i].z * -1
};
}
return new CvMat(pointsCount, 3, MatrixType.F32C1, objects);
}
private void buttonTest0_Click(object sender, RoutedEventArgs e)
{
int cols, rows;
double horizLength, vertLength;
if (!parseChessboardParameters(out cols, out rows, out horizLength, out vertLength))
{
return;
}
// 以下修正
MotionDataHandler handler;
string path;
if (openMotionData(out handler, out path))
{
CvMat displayMat1 = null;
CvMat displayMat3 = null;
CvMat displayMat4 = null;
CvMat gray = null;
int length = handler.FrameCount;
if (length == 0) { return; }
CvSize boardSize = new CvSize(cols, rows);
CvSize imageSize = new CvSize();
double minVarDistance2d = double.MaxValue;
IEnumerable<CvMat> colorImages, depthImages;
Utility.LoadImages(handler.GetColorImagePaths(), out colorImages);
Utility.LoadImages(handler.GetDepthImagePaths(), out depthImages);
var images = colorImages.Zip(depthImages, (first, second) => Tuple.Create(first, second));
foreach (Tuple<CvMat, CvMat> imagePair in images)
{
CvMat imageMat = imagePair.Item1;
CvMat depthMat = imagePair.Item2;
if (displayMat4 == null)
{
displayMat4 = CvEx.InitCvMat(imageMat);
}
imageSize = new CvSize(imageMat.Cols, imageMat.Rows);
CvSize depthUserSize = new CvSize(depthMat.Cols, depthMat.Rows);
CvPoint2D32f[] corners;
int count;
CvEx.InitCvMat(ref gray, imageMat, MatrixType.U8C1);
imageMat.CvtColor(gray, ColorConversion.RgbToGray);
if (gray.FindChessboardCorners(boardSize, out corners, out count, ChessboardFlag.AdaptiveThresh))
{
CvEx.CloneCvMat(ref displayMat1, imageMat);
CvTermCriteria criteria = new CvTermCriteria(50, 0.01);
gray.FindCornerSubPix(corners, count, new CvSize(3, 3), new CvSize(-1, -1), criteria);
CvPoint3D32f?[] cornerPoints = new CvPoint3D32f?[corners.Length];
for (int j = 0; j < corners.Length; j++)
{
CvPoint2D32f corner = new CvPoint2D32f(corners[j].X - 10, corners[j].Y - 10);
double? value = CvEx.Get2DSubPixel(depthMat, corner, 0);
if (value.HasValue)
{
double depth = UndistortionData.UndistortDepth(corner.X, corner.Y, value.Value, depthUserSize);
cornerPoints[j] = new CvPoint3D32f(corner.X, corner.Y, depth);
}
}
List<double> distance2dList = new List<double>();
for (int x = 0; x < cols; x++)
{
for (int y = 0; y < rows; y++)
{
if (!cornerPoints[x + y * cols].HasValue)
continue;
int nextX = x + 1;
if (nextX < cols)
{
if (!cornerPoints[nextX + y * cols].HasValue)
continue;
CvPoint3D32f point = cornerPoints[x + y * cols].Value;
CvPoint3D32f nextPoint = cornerPoints[nextX + y * cols].Value;
distance2dList.Add(Math.Sqrt(Math.Pow(point.X - nextPoint.X, 2) + Math.Pow(point.Y - nextPoint.Y, 2)));
}
int nextY = y + 1;
if (nextY < rows)
{
if (!cornerPoints[x + nextY * cols].HasValue)
continue;
CvPoint3D32f point = cornerPoints[x + y * cols].Value;
CvPoint3D32f nextPoint = cornerPoints[x + nextY * cols].Value;
distance2dList.Add(Math.Sqrt(Math.Pow(point.X - nextPoint.X, 2) + Math.Pow(point.Y - nextPoint.Y, 2)));
}
}
}
if (distance2dList.Count >= 2)
{
double stdevDistance2d = CalcEx.GetStdDev(distance2dList);
displayMat1.PutText(string.Format("{0:0.00}/{1:0.00}", stdevDistance2d, minVarDistance2d), new CvPoint(0, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 0));
double avgDepth = cornerPoints.Where(p => p.HasValue).Select(p => p.Value.Z).Average();
for (int x = 0; x < cols; x++)
{
for (int y = 0; y < rows; y++)
{
if (!cornerPoints[x + y * cols].HasValue)
continue;
CvPoint3D32f point = cornerPoints[x + y * cols].Value;
displayMat1.PutText((point.Z - avgDepth).ToString("0.00"), new CvPoint((int)point.X, (int)point.Y), new CvFont(FontFace.HersheyPlain, 0.6, 0.6), new CvScalar(255, 0, 0));
displayMat1.PutText(((point.Z - avgDepth) / avgDepth * 100).ToString("0.000"), new CvPoint((int)point.X, (int)point.Y + 12), new CvFont(FontFace.HersheyPlain, 0.6, 0.6), new CvScalar(0, 255, 255));
}
}
//displayMat1.DrawChessboardCorners(boardSize, corners, true);
if (stdevDistance2d < minVarDistance2d)
{
minVarDistance2d = stdevDistance2d;
CvEx.CloneCvMat(ref displayMat4, displayMat1);
}
//System.Threading.Thread.Sleep(500);
}
putImage(displayMat1, PixelFormats.Rgb24);
}
else
{
CvEx.CloneCvMat(ref displayMat3, imageMat);
putImage(displayMat3, PixelFormats.Rgb24);
}
}
putImage(displayMat4, PixelFormats.Rgb24);
displayLabels();
}
}
private void buttonScalingScore_Click(object sender, RoutedEventArgs e)
{
int cols, rows;
double horizLength, vertLength;
if (!parseChessboardParameters(out cols, out rows, out horizLength, out vertLength))
{
return;
}
// 以下改造
MotionDataHandler handler;
string path;
if (openMotionData(out handler, out path))
{
CvMat displayMat1 = null;
CvMat displayMat3 = null;
CvMat displayMat4 = null;
CvMat gray = null;
int length = handler.FrameCount;
if (length == 0) { return; }
CvSize boardSize = new CvSize(cols, rows);
CvSize imageSize = new CvSize();
List<Tuple<double, double>> pairs = new List<Tuple<double, double>>();
CvPoint2D32f[] lastCorners = null;
IEnumerable<CvMat> colorImages, depthImages;
Utility.LoadImages(handler.GetColorImagePaths(), out colorImages);
Utility.LoadImages(handler.GetDepthImagePaths(), out depthImages);
var images = colorImages.Zip(depthImages, (first, second) => Tuple.Create(first, second));
foreach (Tuple<CvMat, CvMat> imagePair in images)
{
CvMat imageMat = imagePair.Item1;
CvMat depthMat = imagePair.Item2;
if (displayMat4 == null)
{
displayMat4 = CvEx.InitCvMat(imageMat);
}
imageSize = new CvSize(imageMat.Cols, imageMat.Rows);
CvPoint2D32f[] corners;
int count;
CvEx.InitCvMat(ref gray, imageMat, MatrixType.U8C1);
imageMat.CvtColor(gray, ColorConversion.RgbToGray);
if (gray.FindChessboardCorners(boardSize, out corners, out count, ChessboardFlag.AdaptiveThresh))
{
CvEx.CloneCvMat(ref displayMat1, imageMat);
CvTermCriteria criteria = new CvTermCriteria(50, 0.01);
gray.FindCornerSubPix(corners, count, new CvSize(3, 3), new CvSize(-1, -1), criteria);
CvPoint3D32f?[] cornerPoints = new CvPoint3D32f?[corners.Length];
for (int j = 0; j < corners.Length; j++)
{
CvPoint2D32f corner = corners[j];
double? value = CalcEx.BilateralFilterDepthMatSinglePixel(corner, depthMat, 100, 4, 9);
if (value.HasValue)
{
cornerPoints[j] = new CvPoint3D32f(corner.X, corner.Y, value.Value);
}
}
for (int x = 0; x < cols; x++)
{
for (int y = 0; y < rows; y++)
{
if (!cornerPoints[x + y * cols].HasValue)
continue;
CvPoint3D32f point1 = cornerPoints[x + y * cols].Value;
CvPoint3D64f undistortPoint1 = this.UndistortionData.GetRealFromScreenPos(point1, imageSize);
foreach (var offset in new[] { new { X = 1, Y = 0, D = horizLength }, new { X = 0, Y = 1, D = vertLength } })
{
int dx = x + offset.X;
int dy = y + offset.Y;
if (dx >= cols || dy >= rows)
continue;
if (!cornerPoints[dx + dy * cols].HasValue)
continue;
CvPoint3D32f point2 = cornerPoints[dx + dy * cols].Value;
CvPoint3D64f undistortPoint2 = this.UndistortionData.GetRealFromScreenPos(point2, imageSize);
double distance = Math.Sqrt(CvEx.GetDistanceSq(undistortPoint1, undistortPoint2));
double scale = distance / offset.D;
CvColor color = CalcEx.HSVtoRGB(Math.Max(0, Math.Min(300, scale * 600 - 450)), scale, 2 - scale);
displayMat4.DrawLine((int)point1.X, (int)point1.Y, (int)point2.X, (int)point2.Y, new CvScalar(color.R, color.G, color.B), 1, LineType.AntiAlias);
pairs.Add(new Tuple<double, double>(distance, offset.D));
}
}
}
CvEx.DrawChessboardCornerFrame(displayMat1, boardSize, corners, new CvScalar(64, 128, 64));
displayMat1.DrawChessboardCorners(boardSize, corners, true);
lastCorners = corners;
putImage(displayMat1, PixelFormats.Rgb24);
}
else
{
CvEx.CloneCvMat(ref displayMat3, imageMat);
putImage(displayMat3, PixelFormats.Rgb24);
}
}
CvMat displayMat2 = CvEx.InitCvMat(displayMat1);
displayMat1.Undistort2(displayMat2, this.UndistortionData.CameraStruct.CreateCvMat(), this.UndistortionData.DistortStruct.CreateCvMat(true));
if (lastCorners != null)
{
drawUndistortedCornerFrame(displayMat2, lastCorners, boardSize);
}
displayMat2.PutText(string.Format("Min: {0}", pairs.Min(x => x.Item1 / x.Item2)), new CvPoint(20, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Max: {0}", pairs.Max(x => x.Item1 / x.Item2)), new CvPoint(20, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Avg: {0}", pairs.Average(x => x.Item1 / x.Item2)), new CvPoint(20, 60), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Med: {0}", CalcEx.GetMedian(pairs.Select(x => x.Item1 / x.Item2).ToList())), new CvPoint(20, 80), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
putImage(displayMat4, PixelFormats.Rgb24);
displayLabels();
}
}
private void buttonCalibrateScaleOffset_Click(object sender, RoutedEventArgs e)
{
int cols, rows;
double horizLength, vertLength;
if (!parseChessboardParameters(out cols, out rows, out horizLength, out vertLength))
{
return;
}
// 以下改造
MotionDataHandler handler;
string path;
if (openMotionData(out handler, out path))
{
CvMat displayMat1 = null;
CvMat displayMat3 = null;
CvMat gray = null;
if (ProgressData.DoAction(progress =>
{
int length = handler.FrameCount;
if (length == 0) { return; }
progress.InitProgress("Find Chessboard...", length * 2);
CvSize boardSize = new CvSize(cols, rows);
List<CvPoint3D32f?[]> list = new List<CvPoint3D32f?[]>();
CvSize imageSize = new CvSize();
CvPoint2D32f[] lastCorners = null;
IEnumerable<CvMat> colorImages, depthImages;
Utility.LoadImages(handler.GetColorImagePaths(), out colorImages);
Utility.LoadImages(handler.GetDepthImagePaths(), out depthImages);
var images = colorImages.Zip(depthImages, (first, second) => Tuple.Create(first, second));
foreach (Tuple<CvMat, CvMat> imagePair in images)
{
progress.CurrentValue++;
CvMat imageMat = imagePair.Item1;
CvMat depthMat = imagePair.Item2;
imageSize = new CvSize(imageMat.Cols, imageMat.Rows);
CvPoint2D32f[] corners;
int count;
CvEx.InitCvMat(ref gray, imageMat, MatrixType.U8C1);
imageMat.CvtColor(gray, ColorConversion.RgbToGray);
if (gray.FindChessboardCorners(boardSize, out corners, out count, ChessboardFlag.AdaptiveThresh))
{
CvEx.CloneCvMat(ref displayMat1, imageMat);
CvTermCriteria criteria = new CvTermCriteria(50, 0.01);
gray.FindCornerSubPix(corners, count, new CvSize(3, 3), new CvSize(-1, -1), criteria);
CvPoint3D32f?[] cornerPoints = new CvPoint3D32f?[corners.Length];
for (int j = 0; j < corners.Length; j++)
{
CvPoint2D32f corner = corners[j];
double? value = CalcEx.BilateralFilterDepthMatSinglePixel(corner, depthMat, 100, 4, 9);
if (value.HasValue)
{
cornerPoints[j] = new CvPoint3D32f(corner.X, corner.Y, value.Value);
}
}
list.Add(cornerPoints);
CvEx.DrawChessboardCornerFrame(displayMat1, boardSize, corners, new CvScalar(64, 128, 64));
displayMat1.DrawChessboardCorners(boardSize, corners, true);
lastCorners = corners;
//putImage(displayMat1, PixelFormats.Bgr24);
}
else
{
CvEx.CloneCvMat(ref displayMat3, imageMat);
//putImage(displayMat3, PixelFormats.Bgr24);
}
}
progress.SetProgress("Scale Offset Calibrating...", length);
this.UndistortionData.CalibrateRealScaleAndOffset(list, cols, rows, horizLength, vertLength, imageSize);
CvMat displayMat2 = CvEx.InitCvMat(displayMat1);
displayMat1.Undistort2(displayMat2, this.UndistortionData.CameraStruct.CreateCvMat(), this.UndistortionData.DistortStruct.CreateCvMat(true));
if (lastCorners != null)
{
drawUndistortedCornerFrame(displayMat2, lastCorners, boardSize);
}
displayMat2.PutText(string.Format("XScale: {0}", this.UndistortionData.XScale), new CvPoint(20, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("YScale: {0}", this.UndistortionData.YScale), new CvPoint(20, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Zoffset: {0}", this.UndistortionData.ZOffset), new CvPoint(20, 60), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
putImage(displayMat2, PixelFormats.Bgr24);
}, "Calibrate Scale Offset", true))
{
displayLabels();
}
}
}
public CalibrateCamera()
{
const int ImageNum = 3;
const int PatRow = 7;
const int PatCol = 10;
const int PatSize = PatRow * PatCol;
const int AllPoints = ImageNum * PatSize;
const float ChessSize = 24.0f;
IplImage[] srcImg = new IplImage[ImageNum];
for (int i = 0; i < ImageNum; i++)
{
srcImg[i] = new IplImage(string.Format(FilePath.Image.Calibration, i), LoadMode.Color);
}
CvPoint3D32f[,,] objects = new CvPoint3D32f[ImageNum, PatRow, PatCol];
for (int i = 0; i < ImageNum; i++)
{
for (int j = 0; j < PatRow; j++)
{
for (int k = 0; k < PatCol; k++)
{
objects[i, j, k] = new CvPoint3D32f
{
X = j * ChessSize,
Y = k * ChessSize,
Z = 0.0f
};
}
}
}
CvMat objectPoints = new CvMat(AllPoints, 3, MatrixType.F32C1, objects);
CvSize patternSize = new CvSize(PatCol, PatRow);
int foundNum = 0;
List <CvPoint2D32f> allCorners = new List <CvPoint2D32f>(AllPoints);
int[] pointCountsValue = new int[ImageNum];
using (CvWindow window = new CvWindow("Calibration", WindowMode.AutoSize))
{
for (int i = 0; i < ImageNum; i++)
{
CvPoint2D32f[] corners;
bool found = Cv.FindChessboardCorners(srcImg[i], patternSize, out corners);
Debug.Print("{0:D2}...", i);
if (found)
{
Debug.Print("ok");
foundNum++;
}
else
{
Debug.Print("fail");
}
using (IplImage srcGray = new IplImage(srcImg[i].Size, BitDepth.U8, 1))
{
Cv.CvtColor(srcImg[i], srcGray, ColorConversion.BgrToGray);
Cv.FindCornerSubPix(srcGray, corners, corners.Length, new CvSize(3, 3), new CvSize(-1, -1), new CvTermCriteria(20, 0.03));
Cv.DrawChessboardCorners(srcImg[i], patternSize, corners, found);
pointCountsValue[i] = corners.Length;
window.ShowImage(srcImg[i]);
Cv.WaitKey(0);
}
allCorners.AddRange(corners);
}
if (foundNum != ImageNum)
{
Debug.Assert(false);
}
}
CvMat imagePoints = new CvMat(AllPoints, 1, MatrixType.F32C2, allCorners.ToArray());
CvMat pointCounts = new CvMat(ImageNum, 1, MatrixType.S32C1, pointCountsValue);
CvMat intrinsic = new CvMat(3, 3, MatrixType.F64C1);
CvMat distortion = new CvMat(1, 4, MatrixType.F64C1);
CvMat rotation = new CvMat(ImageNum, 3, MatrixType.F64C1);
CvMat translation = new CvMat(ImageNum, 3, MatrixType.F64C1);
Cv.CalibrateCamera2(objectPoints, imagePoints, pointCounts, srcImg[0].Size, intrinsic, distortion, rotation, translation, CalibrationFlag.Default);
CvMat subImagePoints, subObjectPoints;
Cv.GetRows(imagePoints, out subImagePoints, 0, PatSize);
Cv.GetRows(objectPoints, out subObjectPoints, 0, PatSize);
CvMat rotation_ = new CvMat(1, 3, MatrixType.F32C1);
CvMat translation_ = new CvMat(1, 3, MatrixType.F32C1);
Cv.FindExtrinsicCameraParams2(subObjectPoints, subImagePoints, intrinsic, distortion, rotation_, translation_, false);
//Cv.FindExtrinsicCameraParams2_(subObjectPoints, subImagePoints, intrinsic, distortion, rotation_, translation_, false);
using (var fs = new CvFileStorage("camera.xml", null, OpenCvSharp.FileStorageMode.Write))
{
fs.Write("intrinsic", intrinsic);
fs.Write("rotation", rotation_);
fs.Write("translation", translation_);
fs.Write("distortion", distortion);
}
foreach (IplImage img in srcImg)
{
img.Dispose();
}
// 書き込んだファイルを表示
Console.WriteLine(File.ReadAllText("camera.xml"));
Console.Read();
}
/// <summary>
///
/// </summary>
/// <param name="point"></param>
#else
/// <summary>
///
/// </summary>
/// <param name="point"></param>
#endif
public CvPoint3D64f PerpendicularFoot(CvPoint3D32f point)
{
return PerpendicularFoot(point.X, point.Y, point.Z);
}
public CalibrateStereoCamera()
{
// target filenames
string[] pair = { "Data/Image/Calibration/left{0:D2}.jpg", "Data/Image/Calibration/right{0:D2}.jpg" };
string[][] fileNames = new string[ImageNum][];
for (int i = 0; i < ImageNum; i++)
{
fileNames[i] = new string[2] {
string.Format(pair[0], i + 1), string.Format(pair[1], i + 1)
};
}
// FindChessboardCorners
CvPoint2D32f[] imagePointsLeft, imagePointsRight;
int[] pointCountLeft, pointCountRight;
int[] goodImagelist;
FindChessboardCorners(fileNames, out imagePointsLeft, out imagePointsRight, out pointCountLeft, out pointCountRight, out goodImagelist);
int nImages = goodImagelist.Length;
// StereoCalibrate
CvPoint3D32f[, ,] objects = new CvPoint3D32f[ImageNum, BoardSize.Height, BoardSize.Width];
for (int i = 0; i < ImageNum; i++)
{
for (int j = 0; j < BoardSize.Height; j++)
{
for (int k = 0; k < BoardSize.Width; k++)
{
objects[i, j, k] = new CvPoint3D32f(j * SquareSize, k * SquareSize, 0.0f);
}
}
}
CvMat objectPoints = new CvMat(AllPoints, 3, MatrixType.F32C1, objects);
CvMat imagePoints1 = new CvMat(AllPoints, 1, MatrixType.F32C2, imagePointsLeft);
CvMat imagePoints2 = new CvMat(AllPoints, 1, MatrixType.F32C2, imagePointsRight);
CvMat pointCount1 = new CvMat(nImages, 1, MatrixType.S32C1, pointCountLeft);
CvMat pointCount2 = new CvMat(nImages, 1, MatrixType.S32C1, pointCountRight);
CvMat cameraMatrix1 = CvMat.Identity(3, 3, MatrixType.F64C1);
CvMat cameraMatrix2 = CvMat.Identity(3, 3, MatrixType.F64C1);
CvMat distCoeffs1 = new CvMat(1, 4, MatrixType.F64C1);
CvMat distCoeffs2 = new CvMat(1, 4, MatrixType.F64C1);
CvMat R = new CvMat(3, 3, MatrixType.F64C1);
CvMat T = new CvMat(3, 1, MatrixType.F64C1);
Cv.StereoCalibrate(objectPoints, imagePoints1, imagePoints2, pointCount1,
cameraMatrix1, distCoeffs1,
cameraMatrix2, distCoeffs2,
new CvSize(640, 480), R, T, null, null,
new CvTermCriteria(100, 1e-5),
CalibrationFlag.FixAspectRatio | CalibrationFlag.ZeroTangentDist | CalibrationFlag.SameFocalLength |
CalibrationFlag.RationalModel | CalibrationFlag.FixK3 | CalibrationFlag.FixK4 | CalibrationFlag.FixK5);
// Rectify
CvMat R1 = new CvMat(3, 3, MatrixType.F64C1);
CvMat R2 = new CvMat(3, 3, MatrixType.F64C1);
CvMat P1 = new CvMat(3, 4, MatrixType.F64C1);
CvMat P2 = new CvMat(3, 4, MatrixType.F64C1);
CvMat Q = new CvMat(4, 4, MatrixType.F64C1);
Cv.StereoRectify(cameraMatrix1, cameraMatrix2, distCoeffs1, distCoeffs2,
new CvSize(640, 480), R, T, R1, R2, P1, P2, Q,
StereoRectificationFlag.ZeroDisparity, 1, new CvSize(640, 480));
using (CvMemStorage mem = new CvMemStorage())
using (CvFileStorage fs = new CvFileStorage("extrinsic.yml", mem, OpenCvSharp.FileStorageMode.Write))
{
fs.Write("R", R);
fs.Write("T", T);
fs.Write("R1", R1);
fs.Write("R2", R2);
fs.Write("P1", P1);
fs.Write("P1", P1);
fs.Write("Q", Q);
}
Process.Start("notepad", "extrinsic.yml");
Console.Read();
}
/// <summary>
/// 指定した点と直線の距離を返す
/// </summary>
/// <param name="point"></param>
#else
/// <summary>
/// Returns the distance between this line and the specified point
/// </summary>
/// <param name="point"></param>
#endif
public double Distance(CvPoint3D32f point)
{
return Distance(point.X, point.Y, point.Z);
}
