/// <summary> /// Release the managed resources associated with this object /// </summary> protected override void ReleaseManagedResources() { _rectStorage.Dispose(); _vector.Dispose(); base.ReleaseManagedResources(); }
/// <summary> /// /// </summary> /// <param name="calibrationInfo"></param> /// <param name="trackedFeaturesKp"></param> /// <param name="bootstrapKp"></param> /// <param name="p"></param> /// <param name="p1"></param> /// <returns></returns> public static TriangulateAndCheckReprojResult TriangulateAndCheckReproj(CameraCalibrationInfo calibrationInfo, VectorOfKeyPoint trackedFeaturesKp, VectorOfKeyPoint bootstrapKp, Matrix <double> p, Matrix <double> p1) { var result = new TriangulateAndCheckReprojResult(); var trackedFeaturesPoints = Utils.GetPointsVector(trackedFeaturesKp); var bootstrapPoints = Utils.GetPointsVector(bootstrapKp); //undistort var normalizedTrackedPts = new VectorOfPointF(); var normalizedBootstrapPts = new VectorOfPointF(); CvInvoke.UndistortPoints(trackedFeaturesPoints, normalizedTrackedPts, calibrationInfo.Intrinsic, calibrationInfo.Distortion); CvInvoke.UndistortPoints(bootstrapPoints, normalizedBootstrapPts, calibrationInfo.Intrinsic, calibrationInfo.Distortion); //triangulate var pt3Dh = new Mat(); CvInvoke.TriangulatePoints(p, p1, normalizedBootstrapPts, normalizedTrackedPts, pt3Dh); var pt3DhMatrix = new Matrix <float>(pt3Dh.Rows, pt3Dh.Cols, pt3Dh.DataPointer); var pt3DMat = new Mat(); CvInvoke.ConvertPointsFromHomogeneous(pt3DhMatrix.Transpose(), pt3DMat); var pt3D = new Matrix <float>(pt3DMat.Rows, pt3DMat.Cols * pt3DMat.NumberOfChannels, pt3DMat.DataPointer); var statusArray = new byte[pt3D.Rows]; for (int i = 0; i < pt3D.Rows; i++) { statusArray[i] = (pt3D[i, 2] > 0) ? (byte)1 : (byte)0; } var status = new VectorOfByte(statusArray); int count = CvInvoke.CountNonZero(status); double percentage = count / (double)pt3D.Rows; if (percentage > 0.75) { //calculate reprojection var rvec = new VectorOfFloat(new float[] { 0, 0, 0 }); var tvec = new VectorOfFloat(new float[] { 0, 0, 0 }); var reprojectedPtSet1 = new VectorOfPointF(); CvInvoke.ProjectPoints(pt3D, rvec, tvec, calibrationInfo.Intrinsic, calibrationInfo.Distortion, reprojectedPtSet1); double reprojErr = CvInvoke.Norm(reprojectedPtSet1, bootstrapPoints) / bootstrapPoints.Size; if (reprojErr < 5) { statusArray = new byte[bootstrapPoints.Size]; for (int i = 0; i < bootstrapPoints.Size; ++i) { var pointsDiff = Utils.SubstarctPoints(bootstrapPoints[i], reprojectedPtSet1[i]); var vectorOfPoint = new VectorOfPointF(new[] { pointsDiff }); statusArray[i] = CvInvoke.Norm(vectorOfPoint) < 20.0 ? (byte)1 : (byte)0; } status = new VectorOfByte(statusArray); var trackedFeatures3D = new VectorOfPoint3D32F(Utils.Get3dPointsArray(pt3D)); Utils.KeepVectorsByStatus(ref trackedFeaturesKp, ref trackedFeatures3D, status); result.Error = reprojErr; result.TrackedFeatures3D = new VectorOfPoint3D32F(trackedFeatures3D.ToArray()); result.FilteredTrackedFeaturesKp = new VectorOfKeyPoint(trackedFeaturesKp.ToArray()); result.FilteredBootstrapKp = new VectorOfKeyPoint(bootstrapKp.ToArray()); result.Result = true; } rvec.Dispose(); tvec.Dispose(); reprojectedPtSet1.Dispose(); } normalizedTrackedPts.Dispose(); normalizedBootstrapPts.Dispose(); pt3Dh.Dispose(); pt3DhMatrix.Dispose(); pt3DMat.Dispose(); pt3D.Dispose(); status.Dispose(); return(result); }