/// <summary>
/// Calculates the "shifted" Extrinsic Camera Parameters in order to obtain a shifted plane.
/// This is done by multiplying the Extrinsic Matrix by the _plane_shift transformation matrix.
/// The extrinsic Calibration Slide is used to save the extrinsics for different patterns. Because of that,
/// the modified extrinsic need to be created and saved.
/// Since an ExtrinsicCameraParameters Object holds the
/// members "ExtrinsicMatrix" (Type: Matrix), "RotationVector" (Type: RotationVector3D, which represents the
/// rotational Part of the Extrinsic Matrix, and "TranslationVector" (Type: Matrix, which represents the
/// translational Part of the Extrinsic Matrix), a conversion of the RotationalMatrix into a RotationVector3D and
/// the extraction of the translational vector is neccessary.
///
/// The Method "cvRodrigues2" is used to convert a rotational Matrix (3x3-matrix) into a RotationVector3D and vice versa.
/// Additionally the function expects an empty 3x9-matrix to store some partial derivatives (we do not use them!).
/// Using the resulting RotationVector3D and the translational vector (corresponds to the fourth column of the combined
/// rotational and translational matrix), the modified extrinsics can be created using the given object constructor.
/// </summary>
/// <returns></returns>
private ExtrinsicCameraParameters CalculateShiftedECP()
{
if (_last_detected_plane != null && _plane_shift != null)
{
RotationVector3D r_Vector = new RotationVector3D();
Matrix <double> modified_ROTMatrix, modified_Translation;
Matrix <double> jacobian = new Matrix <double>(3, 9);
Matrix extrinsic_Mat = Parsley.Core.Extensions.ConvertToParsley.ToParsley(_last_detected_plane.ExtrinsicMatrix);
Matrix help_matrix = Matrix.Identity(4, 4);
//copy the 3x4 extrinsic matrix into the 4x4 matrix, at the given position (initial row, end row, initial column, end column, Matrix)
help_matrix.SetMatrix(0, 2, 0, 3, extrinsic_Mat);
//multiply with shift-matrix
help_matrix = help_matrix.Multiply(_plane_shift);
//extract rotationalMatrix and convert it to Emgu Matrix type (same parameters as SetMatrix)
modified_ROTMatrix = Parsley.Core.Extensions.ConvertFromParsley.ToEmgu(help_matrix.GetMatrix(0, 2, 0, 2));
//extract translational vector
modified_Translation = Parsley.Core.Extensions.ConvertFromParsley.ToEmgu(help_matrix.GetMatrix(0, 2, 3, 3));
//convert the rotational matrix into the RotationVector3D (r_Vector)
CvInvoke.cvRodrigues2(modified_ROTMatrix.Ptr, r_Vector.Ptr, jacobian.Ptr);
return(new ExtrinsicCameraParameters(r_Vector, modified_Translation));
}
else
{
_logger.Warn("No plane has been detected yet.");
return(null);
}
}
public void TestQuaternion3()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble());
Quaternions q2 = new Quaternions();
q2.AxisAngle = q1.AxisAngle;
double epsilon = 1.0e-8;
EmguAssert.IsTrue(Math.Abs(q1.W - q2.W) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.X - q2.X) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Y - q2.Y) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Z - q2.Z) < epsilon);
RotationVector3D rVec = new RotationVector3D(new double[] { q1.AxisAngle.X, q1.AxisAngle.Y, q1.AxisAngle.Z });
Mat m1 = rVec.RotationMatrix;
Matrix <double> m2 = new Matrix <double>(3, 3);
q1.GetRotationMatrix(m2);
Matrix <double> diff = new Matrix <double>(3, 3);
CvInvoke.AbsDiff(m1, m2, diff);
double norm = CvInvoke.Norm(diff, Emgu.CV.CvEnum.NormType.C);
EmguAssert.IsTrue(norm < epsilon);
Quaternions q4 = q1 * Quaternions.Empty;
//EmguAssert.IsTrue(q4.Equals(q1));
}
public static RotationVector3D toRotationVector(this Matrix <double> mat)
{
var r = new RotationVector3D();
CvInvoke.Rodrigues(mat, r);
return(r);
}
private static double Validate(VectorOfVectorOfPoint3D32F processedObjectPoints, VectorOfVectorOfPointF processedImagePoints, Mat cameraMatrix, Mat distCoeffs, VectorOfPoint3D32F rvecs, VectorOfPoint3D32F tvecs, bool fisheye)
{
double error = 0;
int totalpoints = 0;
if (fisheye)
{
for (int i = 0; i < processedObjectPoints.Size; i++)
{
VectorOfPoint3D32F objectFramePoints = processedObjectPoints[i];
VectorOfPointF imageFramePoints = processedImagePoints[i];
RotationVector3D tvec = new RotationVector3D(new double[] { tvecs[i].X, tvecs[i].Y, tvecs[i].Z });
RotationVector3D rvec = new RotationVector3D(new double[] { rvecs[i].X, rvecs[i].Y, rvecs[i].Z });
VectorOfPointF newImageFramePoints = new VectorOfPointF();
Fisheye.ProjectPoints(objectFramePoints, newImageFramePoints, rvec, tvec, cameraMatrix, distCoeffs);
for (int j = 0; j < newImageFramePoints.Size; j++)
{
PointF x1 = newImageFramePoints[j];
PointF x2 = imageFramePoints[j];
totalpoints++;
error += Math.Pow(x1.X - x2.X, 2) + Math.Pow(x1.Y - x2.Y, 2);
}
}
}
return(Math.Sqrt(error / totalpoints));
}
public void CalibrateCV(ChessBoard cb, out PinholeCamera[] cameras)
{
var worldCoordinates = cb.boardLocalCoordinates_cv;
List <List <Point3f> > worldpoints = new List <List <Point3f> >();
for (int i = 0; i < images.Count; i++)
{
worldpoints.Add(cb.boardLocalCoordinates_cv.ToList());
}
double[,] cameraMat2 = new double[3, 3];
var imagepoints = images.Select(x => x.ImagePoints);
Matrix cameramat = new Matrix(3, 3);
Matrix distcoeffs = new Matrix(4, 1);
Mat[] rvecs, tvecs;
CVI.CalibrateCamera(worldpoints.Select(x => x.ToArray()).ToArray(), imagepoints.ToArray(), images.First().imageSize,
cameramat, distcoeffs, CalibType.Default, new MCvTermCriteria(),
out rvecs, out tvecs);
cameras = new PinholeCamera[images.Count];
for (int i = 0; i < rvecs.Length; i++)
{
var rvec = rvecs[i];
var tvec = tvecs[i];
cameras[i] = new PinholeCamera();
var cam = cameras[i];
var rot = new RotationVector3D();
rvec.CopyTo(rot);
var worldMat = new Matrix4d();
}
for (int i = 0; i < cameras.Length; i++)
{
worldpoints.Add(cb.boardLocalCoordinates_cv.ToList());
}
}
public void updateFromCeres(CeresPointOrient paramblock)
{
var rot3d = new RotationVector3D(paramblock.R_rod);
var mat3 = new Matrix <double>(3, 3, rot3d.RotationMatrix.DataPointer);
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 3; c++)
{
RotationMat[r, c] = mat3[r, c];
}
}
setPos(paramblock.t);
WorldMat = WorldMat.Inverted();
OnPropertyChanged();
}
public void Calibrate(VectorOfVectorOfPointF cornersPoints, Size imageSize, int innerCornersPerChessboardCols,
int innerCornersPerChessboardRows)
{
modelPoints = CreateModelPoints(cornersPoints.Size, innerCornersPerChessboardCols,
innerCornersPerChessboardRows);
var rotationVectors = new VectorOfMat();
var translationVectors = new VectorOfMat();
CvInvoke.CalibrateCamera(modelPoints, cornersPoints, imageSize, cameraMatrix, cameraDistortionCoeffs,
rotationVectors, translationVectors, CalibType.Default, new MCvTermCriteria(10));
translation = new Matrix<double>(translationVectors[0].Rows, translationVectors[0].Cols,
translationVectors[0].DataPointer);
var rotationMatrix = new Matrix<double>(rotationVectors[0].Rows, rotationVectors[0].Cols,
rotationVectors[0].DataPointer);
rotation = new RotationVector3D(new[] {rotationMatrix[0, 0], rotationMatrix[1, 0], rotationMatrix[2, 0]});
}
/*
[Test]
public void TestPointInPolygon()
{
Triangle2D tri = new Triangle2D(
new Point2D<float>(-10, -10),
new Point2D<float>(0, 10),
new Point2D<float>(10, -10));
Rectangle<float> rect = new Rectangle(
new Point2D<float>(0.0f, 0.0f),
10f, 10f);
Point2D<float> p1 = new Point2D<float>(0, 0);
Point2D<float> p2 = new Point2D<float>(-20, -20);
Assert.IsTrue(p1.InConvexPolygon(tri));
Assert.IsTrue(p1.InConvexPolygon(rect));
Assert.IsFalse(p2.InConvexPolygon(tri));
Assert.IsFalse(p2.InConvexPolygon(rect));
}*/
private static float[,] ProjectPoints(float[,] points3D, RotationVector3D rotation, Matrix<double> translation, float focalLength)
{
using (Matrix<float> imagePointMat = new Matrix<float>(points3D.GetLength(0), 2))
{
CvInvoke.cvProjectPoints2(new Matrix<float>(points3D), rotation, translation,
new Matrix<double>(new double[,] {
{focalLength, 0, 0},
{0, focalLength, 0},
{0,0,1}}),
IntPtr.Zero, imagePointMat, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero);
return imagePointMat.Data;
}
}
public void TestRotationMatrix3D()
{
double[] rod = new double[] { 0.2, 0.5, 0.3 };
RotationVector3D rodVec = new RotationVector3D(rod);
RotationVector3D rodVec2 = new RotationVector3D();
rodVec2.RotationMatrix = rodVec.RotationMatrix;
Matrix<double> diff = rodVec - rodVec2;
Assert.IsTrue(diff.Norm < 1.0e-8);
}
public void TestQuaternion3()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble());
Quaternions q2 = new Quaternions();
q2.AxisAngle = q1.AxisAngle;
double epsilon = 1.0e-12;
Assert.Less(Math.Abs(q1.W - q2.W), epsilon);
Assert.Less(Math.Abs(q1.X - q2.X), epsilon);
Assert.Less(Math.Abs(q1.Y - q2.Y), epsilon);
Assert.Less(Math.Abs(q1.Z - q2.Z), epsilon);
RotationVector3D rVec = new RotationVector3D(new double[] { q1.AxisAngle.x, q1.AxisAngle.y, q1.AxisAngle.z });
Matrix<double> m1 = rVec.RotationMatrix;
Matrix<double> m2 = new Matrix<double>(3, 3);
q1.GetRotationMatrix(m2);
Matrix<double> diff = new Matrix<double>(3, 3);
CvInvoke.cvAbsDiff(m1, m2, diff);
double norm = CvInvoke.cvNorm(diff, IntPtr.Zero, Emgu.CV.CvEnum.NORM_TYPE.CV_C, IntPtr.Zero);
Assert.Less(norm, epsilon);
}
/// <summary>
/// Calculates the "shifted" Extrinsic Camera Parameters in order to obtain a shifted plane.
/// This is done by multiplying the Extrinsic Matrix by the _plane_shift transformation matrix.
/// The extrinsic Calibration Slide is used to save the extrinsics for different patterns. Because of that,
/// the modified extrinsic need to be created and saved.
/// Since an ExtrinsicCameraParameters Object holds the
/// members "ExtrinsicMatrix" (Type: Matrix), "RotationVector" (Type: RotationVector3D, which represents the
/// rotational Part of the Extrinsic Matrix, and "TranslationVector" (Type: Matrix, which represents the
/// translational Part of the Extrinsic Matrix), a conversion of the RotationalMatrix into a RotationVector3D and
/// the extraction of the translational vector is neccessary.
///
/// The Method "cvRodrigues2" is used to convert a rotational Matrix (3x3-matrix) into a RotationVector3D and vice versa.
/// Additionally the function expects an empty 3x9-matrix to store some partial derivatives (we do not use them!).
/// Using the resulting RotationVector3D and the translational vector (corresponds to the fourth column of the combined
/// rotational and translational matrix), the modified extrinsics can be created using the given object constructor.
/// </summary>
/// <returns></returns>
private ExtrinsicCameraParameters CalculateShiftedECP()
{
if (_last_detected_plane != null && _plane_shift != null)
{
RotationVector3D r_Vector = new RotationVector3D();
Matrix<double> modified_ROTMatrix, modified_Translation;
Matrix<double> jacobian = new Matrix<double>(3, 9);
Matrix extrinsic_Mat = Parsley.Core.Extensions.ConvertToParsley.ToParsley(_last_detected_plane.ExtrinsicMatrix);
Matrix help_matrix = Matrix.Identity(4, 4);
//copy the 3x4 extrinsic matrix into the 4x4 matrix, at the given position (initial row, end row, initial column, end column, Matrix)
help_matrix.SetMatrix(0, 2, 0, 3, extrinsic_Mat);
//multiply with shift-matrix
help_matrix = help_matrix.Multiply(_plane_shift);
//extract rotationalMatrix and convert it to Emgu Matrix type (same parameters as SetMatrix)
modified_ROTMatrix = Parsley.Core.Extensions.ConvertFromParsley.ToEmgu(help_matrix.GetMatrix(0, 2, 0, 2));
//extract translational vector
modified_Translation = Parsley.Core.Extensions.ConvertFromParsley.ToEmgu(help_matrix.GetMatrix(0, 2, 3, 3));
//convert the rotational matrix into the RotationVector3D (r_Vector)
CvInvoke.cvRodrigues2(modified_ROTMatrix.Ptr, r_Vector.Ptr, jacobian.Ptr);
return (new ExtrinsicCameraParameters(r_Vector, modified_Translation));
}
else
{
_logger.Warn("No plane has been detected yet.");
return null;
}
}
/// <summary>
/// Create the extrinsic camera parameters using the specific rotation and translation matrix
/// </summary>
/// <param name="rotation">The rotation vector</param>
/// <param name="translation">The translation vector</param>
public ExtrinsicCameraParameters(RotationVector3D rotation, Matrix<double> translation)
{
RotationVector = rotation;
TranslationVector = translation;
}
/// <summary>
/// Create the extrinsic camera parameters
/// </summary>
public ExtrinsicCameraParameters()
{
RotationVector = new RotationVector3D();
TranslationVector = new Matrix<double>(3, 1);
}
public void TestQuaternion3()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble());
Quaternions q2 = new Quaternions();
q2.AxisAngle = q1.AxisAngle;
double epsilon = 1.0e-8;
EmguAssert.IsTrue(Math.Abs(q1.W - q2.W) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.X - q2.X) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Y - q2.Y) < epsilon);
EmguAssert.IsTrue(Math.Abs(q1.Z - q2.Z) < epsilon);
RotationVector3D rVec = new RotationVector3D(new double[] { q1.AxisAngle.X, q1.AxisAngle.Y, q1.AxisAngle.Z });
Mat m1 = rVec.RotationMatrix;
Matrix<double> m2 = new Matrix<double>(3, 3);
q1.GetRotationMatrix(m2);
Matrix<double> diff = new Matrix<double>(3, 3);
CvInvoke.AbsDiff(m1, m2, diff);
double norm = CvInvoke.Norm(diff, Emgu.CV.CvEnum.NormType.C);
EmguAssert.IsTrue(norm < epsilon);
Quaternions q4 = q1 * Quaternions.Empty;
//EmguAssert.IsTrue(q4.Equals(q1));
}
