/// <summary>
/// Creates a 'squarewave' above the mesh and presses that squarewave down to the surface of the mesh, figuring out which points are interesting for scanning, and in which order. Optional X values are for handling out of bound vertices which sometimes are generated from the Kinect API.
/// </summary>
/// <param name="vertices">The vertices of a mesh</param>
/// <param name="x_absolute_min"></param>
/// <param name="x_absolute_max"></param>
/// <returns></returns>
public List <VertexIndex> CreatePath(List <Vector3> vertices, float x_absolute_min = float.MaxValue, float x_absolute_max = float.MinValue)
{
BoundingBox b = Extensions.FindBoundingBox(vertices, x_absolute_min, x_absolute_max);
List <Vector3> wave = GenerateSquareWave(b);
PLYHandler.Output(wave, Environment.CurrentDirectory, "WAVE");
return(IdentifyPath(wave, vertices));
}
//Assumes the following pipeline:
// 1: ColorMesh outputted from Kinect API
// 2: ColorMesh turned into CVMesh by KinectFusionizer
// 3: No further edits have been made
public CVMesh Slice(CVMesh meshIn, double y_min, double y_max, bool inverted = true)
{
if (y_min >= y_max)
{
throw new Exception("Minimum value must be lower than maximum value");
}
if (meshIn == null)
{
throw new Exception("Mesh is null");
}
if (meshIn.Vertices.Count == 0)
{
throw new Exception("Mesh has no vertices");
}
CVMesh meshOut = CVMesh.Clone(meshIn);
//the depth image is flipped in comparison to output image
if (inverted)
{
double yMinTemp = y_min;
y_min = y_max;
y_max = yMinTemp;
y_min *= -1;
y_max *= -1;
}
yMin = y_min;
yMax = y_max;
//only add the same vector if it is unique. the vector itself is the key.
//the value is the original index the vector had
meshOut.Faces = Extensions.ToFaces(meshOut.TriangleIndeces);
meshOut = RemoveNonSurfaceFaces(meshOut);
Dictionary <Vector3, int> uniques = new Dictionary <Vector3, int>();
Dictionary <int, int> oldToNewIndices = new Dictionary <int, int>();
//We can only delete something once, but multiple deleted vertices can
//refer to one unique vertice, so:
//Key = deleted, value = unique vertice
Dictionary <int, int> deleted = new Dictionary <int, int>();
SplitVertices(ref uniques, ref deleted, ref oldToNewIndices, meshOut.Vertices, meshOut);
meshOut.Faces = AssignFacesNewIndices(deleted, oldToNewIndices, meshOut.Faces);
meshOut.Vertices = new List <Vector3>();
foreach (var unique in uniques)
{
meshOut.Vertices.Add(unique.Key);
}
meshOut.TriangleIndeces = Extensions.ToTriangleIndices(meshOut.Faces);
string location = Environment.CurrentDirectory;
PLYHandler.Output(meshOut, ref location, false);
return(meshOut);
}
public static float UR_PROBE_OFFSET = 0.10f; //The length of whatever is attached to the tool center point in meters.
/// <summary>
/// Finds robot arm tool center point poses for a list of vertices
/// </summary>
/// <param name="vectorPath">A list of vertices in a mesh. Vertices must exist in mesh and be part of at least one face each</param>
/// <param name="mesh">The mesh that the vectorPath belongs to</param>
/// <returns>A list of URPoses used to feed the robot arm later</returns>
public List <URPose> ToURPath(List <VertexIndex> vectorPath, CVMesh mesh)
{
List <URPose> poses = new List <URPose>();
foreach (var v in vectorPath)
{
Vector3 vertexNormal = FindVertexNormal(v, mesh);
//vertexNormal.X = 0.00001f;
vertexNormal = Extensions.Normalize(vertexNormal);
Vector3 urPos = FindURPosition(v.Vector, vertexNormal);
//Vector3 smoothedNormal = Extensions.Normalize(Extensions.AvgVector(new List<Vector3> {vertexNormal, new Vector3 {X=0, Y=0, Z=1}}));
Vector3 rotationNormal = Extensions.Multiply(vertexNormal, -1);
//Vector3 urRot = ToRotationVector(rotationNormal);
URPose pose = new URPose(urPos.X, urPos.Y, urPos.Z, rotationNormal.X, rotationNormal.Y, rotationNormal.Z);
poses.Add(pose);
}
int percent = (int)Math.Floor(poses.Count * 0.12);
var snippet = poses.GetRange(percent, poses.Count - percent * 2);
PLYHandler.Output(snippet, Environment.CurrentDirectory);
ConvertDirectionVectors(snippet);
return(snippet);
}
