diff --git a/CMakeLists.txt b/CMakeLists.txt index 84eed78..6f49aa0 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/src/landmark_tools/landmark_util/point_cloud2grid.c b/src/landmark_tools/landmark_util/point_cloud2grid.c index ac8dd28..7673f78 100644 --- a/src/landmark_tools/landmark_util/point_cloud2grid.c +++ b/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 // for bool, false +#include // for int32_t +#include // for printf +#include // for free +#include // for strncpy + +#include "landmark_tools/landmark_util/landmark.h" // for free_lmk +#include "landmark_tools/map_projection/datum_conversion.h" // for Planet +#include "landmark_tools/landmark_util/point_cloud2grid.h" +#include "landmark_tools/utils/parse_args.h" // for m_getarg +#include "landmark_tools/map_projection/stereographic_projection.h" + +int is_little_endian() { + uint16_t x = 1; + return *((uint8_t *)&x); +} + +#if defined(MAC_OS) + +#include // for htonl, ntohl, htons, ntohs + +#include +#define letohll(x) OSSwapLittleToHostInt64(x) + +#endif + +#if defined(LINUX_OS) + +#include +#define letohll(x) le64toh(x) + +#endif + +#ifdef WINDOWS_OS + +#include +#define letohll(x) (is_little_endian() ? (x) : _byteswap_uint64(x)) + +#endif + +void show_usage_and_exit() +{ + printf("Make a PLY point cloud from file. \n"); + printf("Input file format is binary double precision, 4 columns: X,Y,Z,ID where X,Y are the stereographic coordinates (in meters), Z is the surface height (in meters) and ID is the LOLA RDR ID.\n"); + printf("Usage for XYZID_2_point:\n"); + printf("------------------\n"); + printf(" Required arguments:\n"); + printf(" -input - input point file.\n"); + printf(" -output - output ply point file \n"); + printf(" -filetype \n"); + printf(" -projection_latitude - Latitude of natural origin\n"); + printf(" -projection_longitude - Reference_Meridian\n"); + printf(" -planet \n"); + exit(EXIT_FAILURE); +} + + +int32_t main(int32_t argc, char **argv) +{ + char *infile= NULL; + char *outfile = NULL; + char *filetype_str = NULL; + double lat0; + double long0; + char * planet_str = NULL; + + argc--; + argv++; + + if (argc==0) show_usage_and_exit(); + + while (argc>0) + { + if (argc==1) show_usage_and_exit(); + //TODO make planet and filetype optional + if ((m_getarg(argv, "-input", &infile, CFO_STRING)!=1) && + (m_getarg(argv, "-output", &outfile, CFO_STRING)!=1) && + (m_getarg(argv, "-filetype", &filetype_str, CFO_STRING)!=1) && + (m_getarg(argv, "-projection_latitude", &lat0, CFO_DOUBLE) != 1) && + (m_getarg(argv, "-projection_longitude", &long0, CFO_DOUBLE) != 1) && + (m_getarg(argv, "-planet", &planet_str, CFO_STRING) != 1)) + show_usage_and_exit(); + + argc-=2; + argv+=2; + } + + if(filetype_str == NULL || infile == NULL || outfile == NULL || planet_str == NULL){ + show_usage_and_exit(); + } + enum e_ply_storage_mode_ filetype = strToPlyFileType(filetype_str); + + enum Planet planet = strToPlanet(planet_str); + if(planet == Planet_UNDEFINED){ + show_usage_and_exit(); + } + + // Read Point Cloud + bool read_success = false; + + double *pts_stereographic; + size_t num_pts; + + if(filetype == PLY_ASCII){ + read_success = readinpoints_xyzid_ascci(infile, &pts_stereographic, &num_pts); + }else if(filetype == PLY_BIG_ENDIAN || filetype == PLY_LITTLE_ENDIAN){ + FILE *fp = fopen(infile, "rb"); + if(fp == NULL) + { + printf("Cannot open file %.256s to read\n", infile); + return EXIT_FAILURE; + } + + // Count points + uint64_t raw; + while (fread(&raw, sizeof(uint64_t), 1, fp) == 1) { + num_pts ++; + } + num_pts = num_pts/4; + pts_stereographic = malloc(num_pts * 3 * sizeof(double)); + if(pts_stereographic == NULL){ + printf("Failure to allocate memory\n"); + return EXIT_FAILURE; + } + + rewind(fp); + + // Read points + size_t i = 0; + size_t point_index = 0; + while (fread(&raw, sizeof(uint64_t), 1, fp) == 1) { + if(point_index > num_pts*3){ + printf("To many points %ld > %ld\n", point_index, num_pts*3); + if(pts_stereographic!=NULL) free(pts_stereographic); + return EXIT_FAILURE; + } + + if(i%4!=3){ + + if(filetype == PLY_BIG_ENDIAN){ + raw = ntohl(raw); // Convert network to host byte order + }else if(filetype == PLY_LITTLE_ENDIAN){ + raw = letohll(raw); // Convert little endian to host byte order + } + memcpy(&(pts_stereographic[point_index]), &raw, sizeof(double)); + point_index ++; + } + i++; + } + + if(point_index == num_pts*3){ + read_success = true; + } + } + + if(!read_success){ + printf("Unable to read %.256s\n", infile); + if(pts_stereographic!=NULL) free(pts_stereographic); + return EXIT_FAILURE; + } + + // Convert from stereographic to ECEF coordinates + double* pts_ecef = malloc(num_pts * 3 * sizeof(double)); + if(pts_ecef == NULL){ + printf("Failure to allocate memory\n"); + if(pts_stereographic!=NULL) free(pts_stereographic); + return EXIT_FAILURE; + } + + for(size_t i = 0; i