Convert xyzid files to ply in world coordinates

CMakeLists.txt

@@ -147,6 +147,15 @@ submodules/librply/src/lib/rply.c
 )
 add_dependencies(point_2_landmark link_public_headers)
 
+add_executable(XYZID_2_point
+src/main/XYZID_2_point_main.c
+${common_sources}
+src/landmark_tools/landmark_util/point_cloud2grid.c
+src/landmark_tools/map_projection/stereographic_projection.c
+submodules/librply/src/lib/rply.c
+)
+add_dependencies(XYZID_2_point link_public_headers)
+
 add_executable(landmark_2_point
 src/main/landmark_2_point_main.c
 ${common_sources}
@@ -191,6 +200,7 @@ endif()
 target_link_libraries( landmark_comparison ${yaml_LIBRARIES} ${PNG_LIBRARIES} ${GSL_LIBRARIES} m -lz)
 target_link_libraries( landmark_registration ${yaml_LIBRARIES} ${PNG_LIBRARIES} ${GSL_LIBRARIES} m -lz)
 target_link_libraries( point_2_landmark ${GSL_LIBRARIES} m)
+target_link_libraries( XYZID_2_point ${GSL_LIBRARIES} m)
 target_link_libraries( landmark_2_point ${GSL_LIBRARIES} m)
 target_link_libraries( distort_landmark ${GSL_LIBRARIES} m)
 target_link_libraries( edit_landmark  ${PNG_LIBRARIES} ${GSL_LIBRARIES} m -lz)

src/landmark_tools/landmark_util/point_cloud2grid.c

@@ -231,6 +231,54 @@ bool readinpoints_ascii(char * plyname, double **pts, uint8_t **bv, size_t *num_
     return true;
 }
 
+bool readinpoints_xyzid_ascci(char * plyname, double **pts, size_t *num_pts)
+{
+    FILE *fp = fopen(plyname, "r");
+    if(fp == NULL)
+    {
+        return false;
+    }
+
+    //Count the number of lines so that we know how much memory to allocate
+    size_t buf_size = 128;
+    char buf[buf_size];
+    memset(&buf, 0, buf_size);
+
+    int32_t line_count;
+    while (fgets(buf, buf_size, fp) != NULL) {
+        line_count++;
+    }
+
+    double *pts_array = malloc(line_count * 3 * sizeof(double));
+    if(pts_array == NULL){
+        printf("Failure to allocate memory\n");
+        if(pts_array!=NULL) free(pts_array);
+        return false;
+    }
+
+    *pts = pts_array;
+
+    rewind(fp);
+
+    // Read each line
+    *num_pts = 0;
+    for(int32_t i = 0; i< line_count; i++){
+        if(fgets(buf, buf_size, fp)){
+            double id = 0;
+            if(sscanf(buf, "%lf %lf %lf %lf", &pts_array[*num_pts*3], &pts_array[*num_pts*3+1], &pts_array[*num_pts*3+2] , &id) == 4){
+                 *num_pts += 1;
+            }else{
+                printf("Failure to scan point values from line %.256s\n", buf);
+                printf("Ignoring line and continuing\n");
+            }
+        }
+    }
+
+    fclose(fp);
+
+    return true;
+}
+
 
 enum PointFileType strToPointFileType(char *str){
     enum PointFileType filetype = POINT;
@@ -255,7 +303,7 @@ enum e_ply_storage_mode_ strToPlyFileType(char *str){
         }else if(strncmp(str, "PLY_BIG_ENDIAN", strlen(str))==0){
             filetype = PLY_BIG_ENDIAN;
         }else if(strncmp(str, "PLY_LITTLE_ENDIAN", strlen(str))==0){
-            filetype = PLY_BIG_ENDIAN;
+            filetype = PLY_LITTLE_ENDIAN;
         }else{
             printf("Value of str must be \"PLY_BIG_ENDIAN\" or \"PLY_LITTLE_ENDIAN\" or \"PLY_ASCII\"\n");
             filetype = PLY_DEFAULT;
@@ -501,3 +549,28 @@ bool Write_LMK_PLY_Points(const char *filename, LMK *lmk, enum e_ply_storage_mod
     if (!ply_close(oply)) return false;
     return true;
 }
+
+bool Write_PLY_Points(const char *filename, double* points, size_t num_points, enum e_ply_storage_mode_ filetype)
+{
+    p_ply oply = ply_create(filename, filetype, NULL, 0, NULL);
+    if (!oply) return false;
+
+    p_ply_element element = NULL;
+
+    if (!ply_add_element(oply, "vertex", num_points)) return false;
+    if (!ply_add_scalar_property(oply, "x", PLY_FLOAT)) return false;
+    if (!ply_add_scalar_property(oply, "y", PLY_FLOAT)) return false;
+    if (!ply_add_scalar_property(oply, "z", PLY_FLOAT)) return false;
+    if (!ply_write_header(oply)) return false;
+
+    // Write point values
+    for(int32_t i = 0; i <num_points; i++)
+    {
+        if (!ply_write(oply, points[i*3])) return false;
+        if (!ply_write(oply, points[i*3 + 1])) return false;
+        if (!ply_write(oply, points[i*3 + 2])) return false;
+    }
+
+    if (!ply_close(oply)) return false;
+    return true;
+}

src/landmark_tools/landmark_util/point_cloud2grid.h

@@ -50,6 +50,7 @@ enum PointFrame{WORLD, LOCAL, RASTER};
  \param[in] bv array of point intensities for each point in `pts`
  \param[in] num_pts number of points in pts
  \param[in,out] lmk should have complete header at the time of input
+ \param[in] frame coordinate frame of input point cloud
  */
 bool point2lmk( double *pts, uint8_t *bv, size_t num_pts, LMK *lmk, enum PointFrame frame);
 
@@ -73,6 +74,15 @@ bool readinply(char * plyname, double **pts, uint8_t **bv, size_t *num_pts);
  */
 bool readinpoints_ascii(char * plyname, double **pts, uint8_t **bv, size_t *num_pts);
 
+/** \brief Open an ascii file containing one point on every line in the form: `X Y Z ID`
+ * \param[in] plyname filename
+ * \param[out] pts coordinate array
+ * \param[out] num_pts number of points in file
+ * \return true if success
+ * \return false if failed to open file
+ */
+bool readinpoints_xyzid_ascci(char * plyname, double **pts, size_t *num_pts);
+
 /**
  \brief TODO
 
@@ -115,6 +125,8 @@ enum PointFrame strToFrame(char *frame_str);
  * \param[in] y0 center row coordinate of landmark
  * \param[in] c number of cols
  * \param[in] r number of rows
+ * \param[in] e_ply_storage_mode_ flag to encode ply as ascii or binary
+ * \param[in] frame coordinate frame for output point cloud
  * \return true on success
  * \return false if fopen cannot open file for writing
  */
@@ -125,6 +137,8 @@ bool Write_LMK_PLY_Facet_Window(const char *filename, LMK *lmk, int32_t x0, int3
  * \brief Write entire landmark to a ply facet
  * \param[in] filename filename for ply facet
  * \param[in] lmk landmark structure
+ * \param[in] e_ply_storage_mode_ flag to encode ply as ascii or binary
+ * \param[in] frame coordinate frame for output point cloud
  * \return true on success
  * \return false if fopen cannot open file for writing
  */
@@ -134,11 +148,24 @@ bool Write_LMK_PLY_Facet(const char *filename, LMK *lmk, enum e_ply_storage_mode
  * \brief Write landmark into a point cloud ply file format
  * \param[in] filename filename for ply facet
  * \param[in] lmk landmark structure
+ * \param[in] e_ply_storage_mode_ flag to encode ply as ascii or binary
+ * \param[in] frame coordinate frame for output point cloud
  * \return true on success
  * \return false if fopen cannot open file for writing
  */
 bool Write_LMK_PLY_Points(const char *filename, LMK *lmk, enum e_ply_storage_mode_ filetype, enum PointFrame frame);
 
+/**
+ * \brief Write a pointcloud into a ply file format
+ * \param[in] filename filename for ply facet
+ * \param[in] pts coordinate array
+ * \param[in] num_pts number of points in file
+ * \param[in] e_ply_storage_mode_ flag to encode ply as ascii or binary
+ * \return true on success
+ * \return false if fopen cannot open file for writing
+ */
+bool Write_PLY_Points(const char *filename, double* points, size_t num_points, enum e_ply_storage_mode_ filetype);
+
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */