finish code except pose

README.md

@@ -27,7 +27,7 @@ clusters = divideIntoFaces(v, f ,n); % clusters have shape [1, #clusters]
 % compile
 mex generatePointsCloud.cpp % -largeArrayDims % This make mxSize size_t
 mex myTraverser.cpp
-pointsPath = generatePathFromClusters(clusters, v, f, n, 0.5, 0);
+[pointsPath, pointsPathIdx] = generatePathFromClusters(clusters, v, f, n, 0.5, 0);
 ```
 The function generatePath will run following code:
 ```matlab
@@ -36,9 +36,6 @@ The function generatePath will run following code:
 ```
 which use graph traverse algorithm, see myTraverser.cpp for detail.
 
-#### Points Cloud generator
-see `generatePointsCloud.cpp`
-
 ## 4. Planning: get the pose along the path
 ### 4.1 Add pose path
 ```matlab

generatePathFromClusters.m

@@ -1,11 +1,11 @@
 %GENERATEPATH generate path
 %
-% path = generatePathFromClusters(clusters, v, f, n), generate path
+% [pointsPath, pointsPathIdx] = generatePathFromClusters(clusters, v, f, n, gap, method), generate path
 % after clusters generated.
 %
 %Author::
 % - JunrZhou
-function pointsPath = generatePathFromClusters(clusters, v, f, n, gap, method)
+function [pointsPath, pointsPathIdx] = generatePathFromClusters(clusters, v, f, n, gap, method)
 if nargin < 6
     method = 0;
 end
@@ -38,10 +38,12 @@
 
 % generate path
 pointsPath = zeros(totalPointsNum, 3);
+pointsPathIdx = zeros(totalPointsNum, 1)
 tmpCount = 0;
 for i=1:cluster_num
     tmp = pathInfoCell{i, 5};
     pointsPath((tmpCount+1):(tmpCount+tmp), :) = pathInfoCell{i, 1};
+    pointsPathIdx((tmpCount+1):(tmpCount+tmp)) = pathInfoCell{i, 2};
     tmpCount = tmpCount+tmp;
 end
 end