internal void CalculateRotation() { m_RotationIncrements.Clear(); List <TrackedFeature> trackedFeatures = m_VisualOdometer.TrackedFeatures; for (int i = 0; i < trackedFeatures.Count; i++) { TrackedFeature trackedFeature = trackedFeatures[i]; if (trackedFeature.Count < 2) { continue; } PointF previousFeatureLocation = trackedFeature[-1]; PointF currentFeatureLocation = trackedFeature[0]; if (currentFeatureLocation.Y <= m_VisualOdometer.SkyRegionBottom) { double previousAngularPlacement = Math.Atan2(previousFeatureLocation.X - m_CenterX, m_FocalLengthX); double currentAngularPlacement = Math.Atan2(currentFeatureLocation.X - m_CenterX, m_FocalLengthX); double rotationIncrement = currentAngularPlacement - previousAngularPlacement; //Debug.WriteLine(headingChange * 180.0 / Math.PI); m_RotationIncrements.Add(rotationIncrement); } } //Debug.WriteLine("Max delta x: " + maxAbsDeltaX); if (m_RotationIncrements.Count > 0) { double meanRotationIncrement = DetermineBestRotationIncrement(); m_HeadingChange = Angle.FromRads(meanRotationIncrement); } }
private void RepopulateFeaturePoints() { System.Drawing.PointF[] newRawTrackedFeaturePoints = this.OpticalFlow.FindFeaturesToTrack( m_CurrentGrayImage, m_TrackedFeatures, m_SkyRegionBottom, m_GroundRegionTop); if (newRawTrackedFeaturePoints.Length == 0) { return; } m_RawTrackedFeaturePoints.AddRange(newRawTrackedFeaturePoints); System.Drawing.PointF[] undistortedNewFeaturePoints = m_CameraParameters.IntrinsicCameraParameters.Undistort( newRawTrackedFeaturePoints, m_CameraParameters.IntrinsicCameraParameters.IntrinsicMatrix, null); for (int i = 0; i < undistortedNewFeaturePoints.Length; i++) { TrackedFeature trackedFeature = new TrackedFeature(); trackedFeature.Add(undistortedNewFeaturePoints[i]); m_TrackedFeatures.Add(trackedFeature); } this.InitialFeaturesCount = m_TrackedFeatures.Count; m_ThresholdForFeatureRepopulation = this.InitialFeaturesCount * 9 / 10; if (m_ThresholdForFeatureRepopulation < 100) { m_ThresholdForFeatureRepopulation = 100; } m_NotTrackedFeaturesCount = 0; }
private void RepopulateFeaturePoints() { System.Drawing.PointF[] newTrackedFeaturePoints = this.OpticalFlow.FindFeaturesToTrack( m_CurrentGrayImage, m_TrackedFeatures, m_SkyRegionBottom, m_GroundRegionTop); for (int i = 0; i < newTrackedFeaturePoints.Length; i++) { TrackedFeature trackedFeature = new TrackedFeature(); trackedFeature.Add(newTrackedFeaturePoints[i]); m_TrackedFeatures.Add(trackedFeature); } this.InitialFeaturesCount = m_TrackedFeatures.Count; m_ThresholdForFeatureRepopulation = this.InitialFeaturesCount * 9 / 10; // We ensure that we don't drop below a fixed limit if (m_ThresholdForFeatureRepopulation < 100) { m_ThresholdForFeatureRepopulation = 100; } m_NotTrackedFeaturesCount = 0; }
private void PopulateRotationCorrectedTranslationIncrements(Angle headingChange) { double s = Math.Sin(headingChange.Rads); double c = Math.Cos(headingChange.Rads); m_TranslationIncrements.Clear(); System.Drawing.PointF[] featurePointPair = new System.Drawing.PointF[2]; List <TrackedFeature> trackedFeatures = m_VisualOdometer.TrackedFeatures; m_GroundFeatures.Clear(); for (int i = 0; i < trackedFeatures.Count; i++) { TrackedFeature trackedFeature = trackedFeatures[i]; if (trackedFeature.Count < 2) { continue; } // previous and current feature points need to be in the ground region if (!(trackedFeature[-1].Y > m_VisualOdometer.GroundRegionTop && trackedFeature[0].Y > m_VisualOdometer.GroundRegionTop)) { continue; } featurePointPair[0] = trackedFeature[-1]; // previous feature location featurePointPair[1] = trackedFeature[0]; // current featue location //Debug.WriteLine("Raw:"); //Debug.WriteLine("\tPrevious dx_r: {0:0.000} dy_r: {1:0.000}", featurePointPair[0].X, featurePointPair[0].Y); //Debug.WriteLine("\tCurrent dx_r: {0:0.000} dy_r: {1:0.000}", featurePointPair[1].X, featurePointPair[1].Y); ProjectOnFloor(featurePointPair); //Debug.WriteLine("Ground:"); //Debug.WriteLine("\tPrevious dx_r: {0:0.000} dy_r: {1:0.000}", featurePointPair[0].X, featurePointPair[0].Y); //Debug.WriteLine("\tCurrent dx_r: {0:0.000} dy_r: {1:0.000}", featurePointPair[1].X, featurePointPair[1].Y); // Remove rotation effect on current feature location. The center of the rotation is the previous feature location Point rotationCorrectedEndPoint = new Point( c * featurePointPair[1].X - s * featurePointPair[1].Y, s * featurePointPair[1].X + c * featurePointPair[1].Y); Point translationIncrement = new Point( featurePointPair[0].X - rotationCorrectedEndPoint.X, featurePointPair[0].Y - rotationCorrectedEndPoint.Y); //double translationAngle = Math.Abs(Math.Atan2(translationIncrement.X, translationIncrement.Y)); ////Debug.WriteLine(translationAngle * 180 / Math.PI); //if (translationAngle > acceptedDirectionMisaligment) //{ // continue; //} //m_UsedGroundFeatures.Add(trackedFeature); m_TranslationIncrements.Add(translationIncrement); m_GroundFeatures.Add(trackedFeature); } }
private void TrackFeatures(Image <Gray, Byte> previousGrayImage) { if (m_RawTrackedFeaturePoints.Count == 0) { return; } OpticalFlowResult opticalFlowResult = this.OpticalFlow.CalculateOpticalFlow(previousGrayImage, m_CurrentGrayImage, m_RawTrackedFeaturePoints.ToArray()); System.Drawing.PointF[] rawTrackedFeaturePoints = opticalFlowResult.TrackedFeaturePoints; m_RawTrackedFeaturePoints.Clear(); m_RawTrackedFeaturePoints.AddRange(rawTrackedFeaturePoints); System.Drawing.PointF[] undistortedNewFeaturePoints = m_CameraParameters.IntrinsicCameraParameters.Undistort( rawTrackedFeaturePoints, m_CameraParameters.IntrinsicCameraParameters.IntrinsicMatrix, null); int fullHistoryFeaturesCount = 0; int unsmoothFeaturesCount = 0; for (int i = undistortedNewFeaturePoints.Length - 1; i >= 0; i--) { bool isTracked = opticalFlowResult.TrackingStatusIndicators[i] == 1; if (isTracked) { TrackedFeature trackedFeature = m_TrackedFeatures[i]; trackedFeature.Add(undistortedNewFeaturePoints[i]); if (trackedFeature.IsFull) { fullHistoryFeaturesCount++; if (!trackedFeature.IsSmooth) { unsmoothFeaturesCount++; } } } else { RemoveTrackedFeature(i); } } if (unsmoothFeaturesCount < fullHistoryFeaturesCount / 2) { // The majority of features is smooth. We downgrade unsmooth features //Debug.WriteLine("Consensus: Is smooth"); ApplyUnsmoothGrades(); } else { // Consensus not smooth; not downgrading unsmooth features. //Debug.WriteLine("Consensus: Is not smooth"); } }
private void TrackFeatures(Image <Gray, Byte> previousGrayImage) { System.Drawing.PointF[] trackedFeaturePoints = new System.Drawing.PointF[m_TrackedFeatures.Count]; for (int i = 0; i < trackedFeaturePoints.Length; i++) { trackedFeaturePoints[i] = m_TrackedFeatures[i][0]; } OpticalFlowResult opticalFlowResult = this.OpticalFlow.CalculateOpticalFlow(previousGrayImage, m_CurrentGrayImage, trackedFeaturePoints); trackedFeaturePoints = opticalFlowResult.TrackedFeaturePoints; int fullHistoryFeaturesCount = 0; int unsmoothFeaturesCount = 0; for (int i = trackedFeaturePoints.Length - 1; i >= 0; i--) { bool isTracked = opticalFlowResult.TrackingStatusIndicators[i] == 1; if (isTracked) { TrackedFeature trackedFeature = m_TrackedFeatures[i]; trackedFeature.Add(trackedFeaturePoints[i]); if (trackedFeature.IsFull) { fullHistoryFeaturesCount++; if (!trackedFeature.IsSmooth) { unsmoothFeaturesCount++; } } } else { RemoveTrackedFeature(i); } } if (unsmoothFeaturesCount < fullHistoryFeaturesCount / 2) { // The majority of features is smooth. We downgrade unsmooth features //Debug.WriteLine("Consensus: Is smooth"); ApplyUnsmoothGrades(); } else { // Consensus not smooth; not downgrading unsmooth features. //Debug.WriteLine("Consensus: Is not smooth"); } }
private void ApplyUnsmoothGrades() { int unsmoothFeaturesOutCount = 0; for (int i = m_TrackedFeatures.Count - 1; i >= 0; i--) { TrackedFeature trackedFeature = m_TrackedFeatures[i]; trackedFeature.ApplyScoreChange(); if (trackedFeature.IsOut) { RemoveTrackedFeature(i); unsmoothFeaturesOutCount++; } } }
private void ApplyUnsmoothGrades() { int unsmoothFeaturesOutCount = 0; for (int i = m_TrackedFeatures.Count - 1; i >= 0; i--) { TrackedFeature trackedFeature = m_TrackedFeatures[i]; trackedFeature.ApplyScoreChange(); if (trackedFeature.IsOut) { RemoveTrackedFeature(i); unsmoothFeaturesOutCount++; } } //Debug.WriteLine("Number of unsmooth features weeded out: " + unsmoothFeaturesOutCount); }
private void PopulateRotationCorrectedTranslationIncrements(Angle headingChange) { double s = Math.Sin(headingChange.Rads); double c = Math.Cos(headingChange.Rads); m_TranslationIncrements.Clear(); System.Drawing.PointF[] featurePointPair = new System.Drawing.PointF[2]; List <TrackedFeature> trackedFeatures = m_VisualOdometer.TrackedFeatures; m_GroundFeatures.Clear(); for (int i = 0; i < trackedFeatures.Count; i++) { TrackedFeature trackedFeature = trackedFeatures[i]; if (trackedFeature.Count < 2) { continue; } // previous and current feature points need to be in the ground region if (!(trackedFeature[-1].Y > m_VisualOdometer.GroundRegionTop && trackedFeature[0].Y > m_VisualOdometer.GroundRegionTop)) { continue; } featurePointPair[0] = trackedFeature[-1]; // previous feature location featurePointPair[1] = trackedFeature[0]; // current featue location ProjectOnFloor(featurePointPair); Point rotationCorrectedEndPoint = new Point( c * featurePointPair[1].X - s * featurePointPair[1].Y, s * featurePointPair[1].X + c * featurePointPair[1].Y); Point translationIncrement = new Point( featurePointPair[0].X - rotationCorrectedEndPoint.X, featurePointPair[0].Y - rotationCorrectedEndPoint.Y); m_TranslationIncrements.Add(translationIncrement); m_GroundFeatures.Add(trackedFeature); } }