public static DenseMatrix DPixelToWorld(DenseVector poseQuaternion,
DenseVector posePosition,
DenseMatrix inverseCalibration,
DenseMatrix homogeneousPixels,
DenseMatrix depths)
{
return ((DenseMatrix)
poseQuaternion.QuaternionToRotation()
.Multiply(DPixelToWorld(inverseCalibration, homogeneousPixels, depths).Translate(posePosition)));
}
public static void TestOpenCVProjection()
{
int numPoints = 1;
Matrix<double> worldPoints = DenseMatrix.CreateRandom(2, numPoints, new ContinuousUniform(100, 300));
worldPoints = worldPoints.InsertRow(2, DenseVector.CreateRandom(numPoints, new ContinuousUniform(500, 2000)));
var posePosition = new DenseVector(new double[] { 1000, 2000, 3000 });
var poseQuat = new DenseVector(new double[] { 0.4823661149, -0.009425591677, 0.8759094477, -0.004083401989 });
//var poseQuat = new DenseVector(new double[] { 1, 0, 0, 0 });
var extParameters = new Emgu.CV.ExtrinsicCameraParameters();
extParameters.RotationVector = new Emgu.CV.RotationVector3D();
extParameters.RotationVector.RotationMatrix = poseQuat.QuaternionToRotation().Inverse().ToEmguMatrix();
extParameters.TranslationVector = (-posePosition).ToColumnMatrix().ToEmguMatrix();
DenseMatrix calibrationMatrix = (DenseMatrix)Pose3D.CreateCalibrationMatrix(525, 320, 240);
var intParameters = new Emgu.CV.IntrinsicCameraParameters();
intParameters.IntrinsicMatrix = calibrationMatrix.ToEmguMatrix();
var openCVProjectedPoints =
Emgu.CV.CameraCalibration.ProjectPoints(
worldPoints.ColumnEnumerator().Select(col => new MCvPoint3D32f((float)col.Item2[0], (float)col.Item2[1], (float)col.Item2[2])).ToArray(),
extParameters,
intParameters);
Console.WriteLine("Open CV Projeted points: \n{0:0.00}", openCVProjectedPoints.ToMatrix(p => new double[] { p.X, p.Y }).Transpose());
var myProjectedPoints = Pose3D.WorldToImagePoints(
worldPoints,
calibrationMatrix,
posePosition,
poseQuat.QuaternionToRotation());
Console.WriteLine("My Projeted points: \n{0:0.00}", myProjectedPoints);
}