public PointF[] GetFeaturesToTrack(Image<Gray, byte> img, VisualOdometerFeaturesToTrackParams parameters)
{
var param = (VisualOdometerFeaturesToTrackParamsST)parameters;
int MaxFeaturesCount = param.MaxFeaturesCount;// = 400;
double QualityLevel = param.QualityLevel;// = 0.01;
double MinDistance = param.MinDistance;// = 1;
int BlockSize = param.BlockSize;// = 10;
var goodFeatures = img.GoodFeaturesToTrack(MaxFeaturesCount, QualityLevel, MinDistance, BlockSize);
return goodFeatures[0];
}
public MCvPoint3D64f? GetTranslationAndRotation(
double[][] rotMatrArray,
OpticFlowFrameContainer prevFrame,
OpticFlowFrameContainer currFrame,
StereoCameraParams cameraParams,
out List<PointF> currFeaturesList,
out List<PointF> prevFeaturesList,
out Matrix<double> resRotation,
VisualOdometerFeaturesToTrackParams featuresToTrackParams,
VisualOdometerFeaturesOpticFlowParams featuresOpticFlowParams,
VisualOdometerDisparitiesParams disparitiesParams)
{
if (
rotMatrArray == null ||
prevFrame == null ||
currFrame == null ||
cameraParams == null)
{
prevFeaturesList = null;
currFeaturesList = null;
resRotation = null;
return null;
}
var leftPrevGrayImg = new Image<Gray, byte>(prevFrame.StereoFrame.LeftRawFrame);
var rightPrevGrayImg = new Image<Gray, byte>(prevFrame.StereoFrame.RightRawFrame);
var leftCurrGrayImg = new Image<Gray, byte>(currFrame.StereoFrame.LeftRawFrame);
var rightCurrGrayImg = new Image<Gray, byte>(currFrame.StereoFrame.RightRawFrame);
var prevFeatures = GetFeaturesToTrack(leftPrevGrayImg, featuresToTrackParams);
PointF[] prevCorrFeatures;
PointF[] currCorrFeatures;
GetFeaturesOpticFlow(prevFeatures, leftPrevGrayImg, leftCurrGrayImg, out prevCorrFeatures, out currCorrFeatures, featuresOpticFlowParams);
var prevDisps = GetDisparities(leftPrevGrayImg, rightPrevGrayImg, prevCorrFeatures, prevFrame.DepthMapImg, disparitiesParams);
var prev3dPoints = ReprojectTo3d(leftPrevGrayImg, prevDisps, prevCorrFeatures, cameraParams.Q);
var currDisps = GetDisparities(leftCurrGrayImg, rightCurrGrayImg, currCorrFeatures, currFrame.DepthMapImg, disparitiesParams);
var curr3dPoints = ReprojectTo3d(leftCurrGrayImg, currDisps, currCorrFeatures, cameraParams.Q);
var prevAct3dPointsList = new List<Matrix<double>>();
var currAct3dPointsList = new List<Matrix<double>>();
for (int i = 0; i < prev3dPoints.Count(); ++i)
{
if (prev3dPoints[i] != null && curr3dPoints[i] != null)
{
prevAct3dPointsList.Add(prev3dPoints[i]);
currAct3dPointsList.Add(curr3dPoints[i]);
}
}
//old reproject
//var depthSize = prevFrame.DepthMapImg.Size;
//var reprojPrevDepthMap = new Image<Gray, short>(prevFrame.DepthMapImg.Size);
//var reprojCurrDepthMap = new Image<Gray, short>(currFrame.DepthMapImg.Size);
//for (int i = 0; i < depthSize.Height; ++i)
//{
// for (int j = 0; j < depthSize.Width; ++j)
// {
// reprojPrevDepthMap.Data[i, j, 0] = 0;
// //reprojPrevDepthMap.Data[i, j, 1] = 0;
// //reprojPrevDepthMap.Data[i, j, 2] = 0;
// reprojCurrDepthMap.Data[i, j, 0] = 0;
// //reprojCurrDepthMap.Data[i, j, 1] = 0;
// //reprojCurrDepthMap.Data[i, j, 2] = 0;
// }
//}
//var actPrevFreatures = new List<PointF>();
//var actCurrFreatures = new List<PointF>();
//for (int i = 0; i < prevFeatures.Count(); ++i)
//{
// if (status[i] == 1)
// {
// actPrevFreatures.Add(prevFeatures[i]);
// actCurrFreatures.Add(currFeatures[i]);
// int xp = (int)prevFeatures[i].X;
// int yp = (int)prevFeatures[i].Y;
// int xc = (int)currFeatures[i].X;
// int yc = (int)currFeatures[i].Y;
// if (yp < reprojPrevDepthMap.Height && yp >=0 && xp < reprojPrevDepthMap.Width && xp >= 0)
// {
// if (yc < reprojCurrDepthMap.Height && yc >= 0 && xc < reprojCurrDepthMap.Width && xc >= 0)
// {
// reprojPrevDepthMap.Data[yp, xp, 0] = prevFrame.DepthMapImg.Data[yp, xp, 0];
// reprojCurrDepthMap.Data[yc, xc, 0] = currFrame.DepthMapImg.Data[yc, xc, 0];
// }
// }
// //reprojPrevDepthMap.Data[yp, xp, 1] = prevFrame.DepthMapImg.Data[yp, xp, 1];
// //reprojPrevDepthMap.Data[yp, xp, 2] = prevFrame.DepthMapImg.Data[yp, xp, 2];
// //reprojCurrDepthMap.Data[yc, xc, 1] = currFrame.DepthMapImg.Data[yc, xc, 1];
// //reprojCurrDepthMap.Data[yc, xc, 2] = currFrame.DepthMapImg.Data[yc, xc, 2];
// }
//}
//var prevPoints = PointCollection.ReprojectImageTo3D(reprojPrevDepthMap, cameraParams.Q);
//var currPoints = PointCollection.ReprojectImageTo3D(reprojCurrDepthMap, cameraParams.Q);
//var maxZ = prevPoints.Max(x => x.z);
//var actPrevPoints = prevPoints.Where(x => x.z != maxZ).ToArray();
//var actCurrPoints = currPoints.Where(x => x.z != maxZ).ToArray();
//// old reproject end
var actPrevPoints = prevAct3dPointsList.ToArray();
var actCurrPoints = currAct3dPointsList.ToArray();
var prevCentroid = GetCentroid(actPrevPoints);
var currCentroid = GetCentroid(actCurrPoints);
var rotMatrix = Utils.CvHelper.ArrayToMatrix(rotMatrArray, new Size(3, 3));
Matrix<double> objRotMatrix;
Matrix<double> resCamRotMatrix;
//rotation with IMU
//var objRotMatrix = Utils.CvHelper.InverseMatrix(rotMatrix);
//rotation with SVD
var detX = GetSVDRotation(actPrevPoints, actCurrPoints, out objRotMatrix);
if (detX < 0)
{
objRotMatrix.SetIdentity();
Console.WriteLine("detX = {0}", detX);
}
resCamRotMatrix = Utils.CvHelper.InverseMatrix(objRotMatrix);
var rotPrevCentr = objRotMatrix.Mul(prevCentroid);
//raw tranalstion
var rawT = new Matrix<double>(rotPrevCentr.Size);
rawT[0, 0] = currCentroid[0, 0] - rotPrevCentr[0, 0];
rawT[1, 0] = currCentroid[1, 0] - rotPrevCentr[1, 0];
rawT[2, 0] = currCentroid[2, 0] - rotPrevCentr[2, 0];
//camera translation
var camT = resCamRotMatrix.Mul(rawT);
//var camT = rawT;
var X = camT[0, 0];
var Y = camT[1, 0];
var Z = camT[2, 0];
prevFeaturesList = prevCorrFeatures.ToList();
currFeaturesList = currCorrFeatures.ToList();
resRotation = resCamRotMatrix;
return new MCvPoint3D64f(X, Y, Z);
}