OpenCVSharp 4.5 单应性矩阵(Homography)估计相机姿态
作者:互联网
用 OpenCVSharp 4.5 跑一遍 OpenCV 官方教程。
原 OpenCV 官方 教程链接:(今天链接失效了?)Demo 1
using System;
using System.Collections.Generic;
using OpenCvSharp;
namespace ConsoleApp1
{
class tutorial52 : ITutorial
{
enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
private void calcChessboardCorners(Size boardSize, float squareSize, out Vec3f[] corners, Pattern patternType = Pattern.CHESSBOARD)
{
List<Vec3f> corners_list = new List<Vec3f>();
//corners.resize(0);
switch (patternType)
{
case Pattern.CHESSBOARD:
case Pattern.CIRCLES_GRID:
//! [compute-chessboard-object-points]
for (int i = 0; i < boardSize.Height; i++)
for (int j = 0; j < boardSize.Width; j++)
corners_list.Add(new Point3f((float)(j * squareSize), (float)(i * squareSize), 0));
//! [compute-chessboard-object-points]
break;
case Pattern.ASYMMETRIC_CIRCLES_GRID:
for (int i = 0; i < boardSize.Height; i++)
for (int j = 0; j < boardSize.Width; j++)
corners_list.Add(new Point3f((float)((2 * j + i % 2) * squareSize), (float)(i * squareSize), 0));
break;
default:
Console.WriteLine("Unknown pattern type\n");
break;
}
corners = corners_list.ToArray();
}
private void poseEstimationFromCoplanarPoints(string imgPath, string intrinsicsPath, Size patternSize, float squareSize)
{
Mat img = Cv2.ImRead(imgPath);
Mat img_corners = img.Clone(), img_pose = img.Clone();
//! [find-chessboard-corners]
Point2f[] corners;
bool found = Cv2.FindChessboardCorners(img, patternSize, out corners);
//! [find-chessboard-corners]
if (found == false)
{
Console.WriteLine("Cannot find chessboard corners.");
return;
}
Cv2.DrawChessboardCorners(img_corners, patternSize, corners, found);
Cv2.ImShow("Chessboard corners detection", img_corners);
//! [compute-object-points]
Vec3f[] objectPoints;
calcChessboardCorners(patternSize, squareSize, out objectPoints);
Point2d[] objectPointsPlanar = new Point2d[objectPoints.Length];
for (int i = 0; i < objectPoints.Length; i++)
{
objectPointsPlanar[i] = (new Point2d(objectPoints[i].Item0, objectPoints[i].Item1));
}
//! [compute-object-points]
//! [load-intrinsics]
Mat cameraMatrix = new Mat(3, 3, MatType.CV_64F, new Double[9] {
5.3591573396163199e+02, 0.0 , 3.4228315473308373e+02 ,
0.0, 5.3591573396163199e+02, 2.3557082909788173e+02 ,
0.0,0.0, 1.0});
Mat distCoeffs = new Mat(5, 1, MatType.CV_64F, new Double[5] { -2.6637260909660682e-01,
-3.8588898922304653e-02,
1.7831947042852964e-03,
-2.8122100441115472e-04,
2.3839153080878486e-01 });
//! [load-intrinsics]
//! [compute-image-points]
Mat imagePoints = new Mat();
Mat corners_mat = Mat.FromArray<Point2f>(corners);
Cv2.UndistortPoints(corners_mat, imagePoints, cameraMatrix, distCoeffs);
//! [compute-image-points]
//! [estimate-homography]
Mat objectPointsPlannar_mat = Mat.FromArray<Point2d>(objectPointsPlanar);
Mat H = Cv2.FindHomography(objectPointsPlannar_mat, imagePoints, HomographyMethods.None);
Console.WriteLine("H:\n{0}", Cv2.Format(H));
//! [estimate-homography]
//! [pose-from-homography]
// Normalization to ensure that ||c1|| = 1
double norm = Math.Sqrt(H.At<double>(0, 0) * H.At<double>(0, 0) +
H.At<double>(1, 0) * H.At<double>(1, 0) +
H.At<double>(2, 0) * H.At<double>(2, 0));
H /= norm;
Mat c1 = H.Col(0);
Mat c2 = H.Col(1);
Mat c3 = c1.Cross(c2);
Mat tvec = H.Col(2);
Mat R = new Mat(3, 3, MatType.CV_64F);
for (int i = 0; i < 3; i++)
{
R.At<double>(i, 0) = c1.At<double>(i, 0);
R.At<double>(i, 1) = c2.At<double>(i, 0);
R.At<double>(i, 2) = c3.At<double>(i, 0);
}
//! [pose-from-homography]
//! [polar-decomposition-of-the-rotation-matrix]
Console.WriteLine("R (before polar decomposition):\n{0}\ndet(R):{1}", Cv2.Format(R), Cv2.Format(Cv2.Determinant(R)));
Mat W = new Mat();
Mat U = new Mat();
Mat Vt = new Mat();
Cv2.SVDecomp(R, W, U, Vt);
R = U * Vt;
Console.WriteLine("R (after polar decomposition):\n{0}\ndet(R):{1}", Cv2.Format(R), Cv2.Format(Cv2.Determinant(R)));
//! [polar-decomposition-of-the-rotation-matrix]
//! [display-pose]
Mat rvec = new Mat();
Cv2.Rodrigues(R, rvec);
Cv2.DrawFrameAxes(img_pose, cameraMatrix, distCoeffs, rvec, tvec, 2 * squareSize);
Cv2.ImShow("Pose from coplanar points", img_pose);
Cv2.WaitKey();
//! [display-pose]
}
public void Run()
{
int width = 9;
int height = 6;
float squareSize = 0.025f;
string imagePath = @"I:\csharp\images\left04.jpg";
string intrinsicsPath = @"\I:\chsarp\images\left_intrinsics.yml";
Size patternSize = new Size(width, height);
poseEstimationFromCoplanarPoints(imagePath, intrinsicsPath, patternSize, squareSize);
return;
}
}
}
标签:4.5,Homography,单应性,img,squareSize,corners,Cv2,new,Mat 来源: https://blog.csdn.net/jimtien/article/details/118883882