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); }