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