/*
* /// <summary>
* /// Create a LevMarqSparse solver
* /// </summary>
* public LevMarqSparse()
* {
* _ptr = CvInvoke.CvCreateLevMarqSparse();
* }*/
/// <summary>
/// Useful function to do simple bundle adjustment tasks
/// </summary>
/// <param name="points">Positions of points in global coordinate system (input and output), values will be modified by bundle adjustment</param>
/// <param name="imagePoints">Projections of 3d points for every camera</param>
/// <param name="visibility">Visibility of 3d points for every camera</param>
/// <param name="cameraMatrix">Intrinsic matrices of all cameras (input and output), values will be modified by bundle adjustment</param>
/// <param name="R">rotation matrices of all cameras (input and output), values will be modified by bundle adjustment</param>
/// <param name="T">translation vector of all cameras (input and output), values will be modified by bundle adjustment</param>
/// <param name="distCoeffcients">distortion coefficients of all cameras (input and output), values will be modified by bundle adjustment</param>
/// <param name="termCrit">Termination criteria, a reasonable value will be (30, 1.0e-12) </param>
public static void BundleAdjust(
MCvPoint3D64f[] points, MCvPoint2D64f[][] imagePoints, int[][] visibility,
Matrix <double>[] cameraMatrix, Matrix <double>[] R, Matrix <double>[] T, Matrix <double>[] distCoeffcients, MCvTermCriteria termCrit)
{
using (Matrix <double> imagePointsMat = CvToolbox.GetMatrixFromPoints(imagePoints))
using (Matrix <int> visibilityMat = CvToolbox.GetMatrixFromArrays(visibility))
using (VectorOfMat cameraMatVec = new VectorOfMat())
using (VectorOfMat rMatVec = new VectorOfMat())
using (VectorOfMat tMatVec = new VectorOfMat())
using (VectorOfMat distorMatVec = new VectorOfMat())
{
cameraMatVec.Push(cameraMatrix);
rMatVec.Push(R);
tMatVec.Push(T);
distorMatVec.Push(distCoeffcients);
GCHandle handlePoints = GCHandle.Alloc(points, GCHandleType.Pinned);
CvInvoke.CvLevMarqSparseAdjustBundle(
cameraMatrix.Length,
points.Length, handlePoints.AddrOfPinnedObject(),
imagePointsMat, visibilityMat, cameraMatVec, rMatVec, tMatVec, distorMatVec, ref termCrit);
handlePoints.Free();
}
}
/// <summary>
/// Create a k-d tree from the specific feature descriptors
/// </summary>
/// <param name="descriptors">The array of feature descriptors</param>
public FeatureTree(float[][] descriptors)
{
_descriptorMatrix = CvToolbox.GetMatrixFromArrays(descriptors);
_ptr = CvInvoke.cvCreateKDTree(_descriptorMatrix.Ptr);
}
/// <summary>
/// Create a spill tree from the specific feature descriptors
/// </summary>
/// <param name="descriptors">The array of feature descriptors</param>
/// <param name="naive">A good value is 50</param>
/// <param name="rho">A good value is .7</param>
/// <param name="tau">A good value is .1</param>
public FeatureTree(float[][] descriptors, int naive, double rho, double tau)
{
_descriptorMatrix = CvToolbox.GetMatrixFromArrays(descriptors);
_ptr = CvInvoke.cvCreateSpillTree(_descriptorMatrix.Ptr, naive, rho, tau);
}
/// <summary>
/// Finds (with high probability) the k nearest neighbors in tree for each of the given (row-)vectors in desc, using best-bin-first searching ([Beis97]). The complexity of the entire operation is at most O(m*emax*log2(n)), where n is the number of vectors in the tree
/// </summary>
/// <param name="descriptors">The m feature descriptors to be searched from the feature tree</param>
/// <param name="results">
/// The results of the best <paramref name="k"/> matched from the feature tree. A m x <paramref name="k"/> matrix. Contains -1 in some columns if fewer than k neighbors found.
/// For each row the k neareast neighbors are not sorted. To findout the closet neighbour, look at the output matrix <paramref name="dist"/>.
/// </param>
/// <param name="dist">
/// A m x <paramref name="k"/> Matrix of the distances to k nearest neighbors
/// </param>
/// <param name="k">The number of neighbors to find</param>
/// <param name="emax">For k-d tree only: the maximum number of leaves to visit. Use 20 if not sure</param>
private void FindFeatures(float[][] descriptors, Matrix <Int32> results, Matrix <double> dist, int k, int emax)
{
using (Matrix <float> descriptorMatrix = CvToolbox.GetMatrixFromArrays(descriptors))
CvInvoke.cvFindFeatures(Ptr, descriptorMatrix.Ptr, results.Ptr, dist.Ptr, k, emax);
}
