From fe409b64b8da384c366938dad8d2a8d727937a4c Mon Sep 17 00:00:00 2001 From: Hari Nair Date: Thu, 24 Apr 2025 15:17:08 -0400 Subject: [PATCH] add support libraries --- support-libraries/.gitattributes | 1 + support-libraries/.gitignore | 6 + support-libraries/README.md | 31 + support-libraries/altwg/CMakeLists.txt | 52 ++ support-libraries/altwg/common/mathutil.h | 188 ++++++ support-libraries/altwg/common/util.h | 70 +++ .../altwg/gravity/README-gravity.txt | 136 +++++ .../altwg/gravity/gravity-cheng.cpp | 135 +++++ .../altwg/gravity/gravity-cheng.h | 10 + .../altwg/gravity/gravity-point.h | 22 + .../altwg/gravity/gravity-werner.cpp | 357 +++++++++++ .../altwg/gravity/gravity-werner.h | 12 + support-libraries/altwg/gravity/gravity.cpp | 561 ++++++++++++++++++ .../altwg/gravity/platemodel.cpp | 213 +++++++ support-libraries/altwg/gravity/platemodel.h | 94 +++ support-libraries/altwg/install-altwg.sh | 95 +++ .../strip-adjustment/closest-point-vtk.cpp | 109 ++++ .../strip-adjustment/closest-point-vtk.h | 10 + .../altwg/strip-adjustment/gradient.ginsh | 67 +++ .../altwg/strip-adjustment/icp-gsl.cpp | 326 ++++++++++ .../altwg/strip-adjustment/icp-gsl.h | 21 + .../altwg/strip-adjustment/lidar-optimize.cpp | 252 ++++++++ .../altwg/strip-adjustment/lidardata.cpp | 203 +++++++ .../altwg/strip-adjustment/lidardata.h | 36 ++ .../altwg/strip-adjustment/optimize-gsl.cpp | 167 ++++++ .../altwg/strip-adjustment/optimize-gsl.h | 15 + .../altwg/strip-adjustment/optimizer.cpp | 72 +++ .../altwg/strip-adjustment/optimizer.h | 51 ++ support-libraries/buildAll.bash | 86 +++ support-libraries/mkPackage.bash | 10 + support-libraries/opencv/install-opencv.sh | 51 ++ support-libraries/spice/install-spice.sh | 102 ++++ support-libraries/util/.gitkeep | 0 .../util/createInfoFiles/Makefile | 74 +++ .../util/createInfoFiles/computePointing.cpp | 470 +++++++++++++++ .../util/createInfoFiles/computePointing.hpp | 14 + .../util/createInfoFiles/createInfoFiles.cpp | 227 +++++++ .../util/createInfoFiles/getFov.cpp | 217 +++++++ .../createInfoFiles/getSpacecraftState.cpp | 59 ++ .../util/createInfoFiles/getTargetState.cpp | 71 +++ .../install-createInfoFiles.sh | 41 ++ .../util/createInfoFiles/saveInfoFile.cpp | 67 +++ support-libraries/vtk/install-vtk.sh | 212 +++++++ .../gluegen-rt-natives-linux-aarch64.jar | Bin 0 -> 3750 bytes .../gluegen-rt-natives-linux-amd64.jar | Bin 0 -> 4345 bytes .../gluegen-rt-natives-linux-armv6hf.jar | Bin 0 -> 3512 bytes .../gluegen-rt-natives-linux-i586.jar | Bin 0 -> 4496 bytes .../gluegen-rt-natives-macosx-universal.jar | Bin 0 -> 6364 bytes .../gluegen-rt-natives-windows-amd64.jar | Bin 0 -> 8709 bytes .../gluegen-rt-natives-windows-i586.jar | Bin 0 -> 7536 bytes .../vtk/resources/gluegen-rt-v2.4.0-rc4.jar | Bin 0 -> 360964 bytes .../jogl-all-natives-linux-aarch64.jar | Bin 0 -> 255353 bytes .../jogl-all-natives-linux-amd64.jar | Bin 0 -> 244529 bytes .../jogl-all-natives-linux-armv6hf.jar | Bin 0 -> 204440 bytes .../resources/jogl-all-natives-linux-i586.jar | Bin 0 -> 264440 bytes .../jogl-all-natives-macosx-universal.jar | Bin 0 -> 463106 bytes .../jogl-all-natives-windows-amd64.jar | Bin 0 -> 227088 bytes .../jogl-all-natives-windows-i586.jar | Bin 0 -> 198933 bytes .../vtk/resources/jogl-all-v2.4.0-rc4.jar | Bin 0 -> 3568895 bytes 59 files changed, 5013 insertions(+) create mode 100644 support-libraries/.gitattributes create mode 100644 support-libraries/.gitignore create mode 100644 support-libraries/README.md create mode 100644 support-libraries/altwg/CMakeLists.txt create mode 100644 support-libraries/altwg/common/mathutil.h create mode 100644 support-libraries/altwg/common/util.h create mode 100644 support-libraries/altwg/gravity/README-gravity.txt create mode 100644 support-libraries/altwg/gravity/gravity-cheng.cpp create mode 100644 support-libraries/altwg/gravity/gravity-cheng.h create mode 100644 support-libraries/altwg/gravity/gravity-point.h create mode 100644 support-libraries/altwg/gravity/gravity-werner.cpp create mode 100644 support-libraries/altwg/gravity/gravity-werner.h create mode 100644 support-libraries/altwg/gravity/gravity.cpp create mode 100644 support-libraries/altwg/gravity/platemodel.cpp create mode 100644 support-libraries/altwg/gravity/platemodel.h create mode 100755 support-libraries/altwg/install-altwg.sh create mode 100644 support-libraries/altwg/strip-adjustment/closest-point-vtk.cpp create mode 100644 support-libraries/altwg/strip-adjustment/closest-point-vtk.h create mode 100644 support-libraries/altwg/strip-adjustment/gradient.ginsh create mode 100644 support-libraries/altwg/strip-adjustment/icp-gsl.cpp create mode 100644 support-libraries/altwg/strip-adjustment/icp-gsl.h create mode 100644 support-libraries/altwg/strip-adjustment/lidar-optimize.cpp create mode 100644 support-libraries/altwg/strip-adjustment/lidardata.cpp create mode 100644 support-libraries/altwg/strip-adjustment/lidardata.h create mode 100644 support-libraries/altwg/strip-adjustment/optimize-gsl.cpp create mode 100644 support-libraries/altwg/strip-adjustment/optimize-gsl.h create mode 100644 support-libraries/altwg/strip-adjustment/optimizer.cpp create mode 100644 support-libraries/altwg/strip-adjustment/optimizer.h create mode 100755 support-libraries/buildAll.bash create mode 100755 support-libraries/mkPackage.bash create mode 100755 support-libraries/opencv/install-opencv.sh create mode 100755 support-libraries/spice/install-spice.sh create mode 100644 support-libraries/util/.gitkeep create mode 100644 support-libraries/util/createInfoFiles/Makefile create mode 100644 support-libraries/util/createInfoFiles/computePointing.cpp create mode 100644 support-libraries/util/createInfoFiles/computePointing.hpp create mode 100644 support-libraries/util/createInfoFiles/createInfoFiles.cpp create mode 100644 support-libraries/util/createInfoFiles/getFov.cpp create mode 100644 support-libraries/util/createInfoFiles/getSpacecraftState.cpp create mode 100644 support-libraries/util/createInfoFiles/getTargetState.cpp create mode 100755 support-libraries/util/createInfoFiles/install-createInfoFiles.sh create mode 100644 support-libraries/util/createInfoFiles/saveInfoFile.cpp create mode 100755 support-libraries/vtk/install-vtk.sh create mode 100644 support-libraries/vtk/resources/gluegen-rt-natives-linux-aarch64.jar create mode 100644 support-libraries/vtk/resources/gluegen-rt-natives-linux-amd64.jar create mode 100644 support-libraries/vtk/resources/gluegen-rt-natives-linux-armv6hf.jar create mode 100644 support-libraries/vtk/resources/gluegen-rt-natives-linux-i586.jar create mode 100644 support-libraries/vtk/resources/gluegen-rt-natives-macosx-universal.jar create mode 100644 support-libraries/vtk/resources/gluegen-rt-natives-windows-amd64.jar create mode 100644 support-libraries/vtk/resources/gluegen-rt-natives-windows-i586.jar create mode 100644 support-libraries/vtk/resources/gluegen-rt-v2.4.0-rc4.jar create mode 100644 support-libraries/vtk/resources/jogl-all-natives-linux-aarch64.jar create mode 100644 support-libraries/vtk/resources/jogl-all-natives-linux-amd64.jar create mode 100644 support-libraries/vtk/resources/jogl-all-natives-linux-armv6hf.jar create mode 100644 support-libraries/vtk/resources/jogl-all-natives-linux-i586.jar create mode 100644 support-libraries/vtk/resources/jogl-all-natives-macosx-universal.jar create mode 100644 support-libraries/vtk/resources/jogl-all-natives-windows-amd64.jar create mode 100644 support-libraries/vtk/resources/jogl-all-natives-windows-i586.jar create mode 100644 support-libraries/vtk/resources/jogl-all-v2.4.0-rc4.jar diff --git a/support-libraries/.gitattributes b/support-libraries/.gitattributes new file mode 100644 index 0000000..c9a6185 --- /dev/null +++ b/support-libraries/.gitattributes @@ -0,0 +1 @@ +*.gz filter=lfs diff=lfs merge=lfs -text diff --git a/support-libraries/.gitignore b/support-libraries/.gitignore new file mode 100644 index 0000000..a93a120 --- /dev/null +++ b/support-libraries/.gitignore @@ -0,0 +1,6 @@ +build/ +dist/ +install/ +spice/src/ +.DS_Store +*.swp diff --git a/support-libraries/README.md b/support-libraries/README.md new file mode 100644 index 0000000..44d918b --- /dev/null +++ b/support-libraries/README.md @@ -0,0 +1,31 @@ +# Supporting libraries for Terrasaur + +The Terrasaur code is available at [GitHub](https://github.com/JHUAPL/Terrasaur.git). This directory contains supporting libraries needed to build Terrasaur. + +* [GSL](https://www.gnu.org/software/gsl/) +* [SPICE](https://naif.jpl.nasa.gov) +* [VTK](https://vtk.org) +* [OpenCV](https://opencv.org/) + +## Prerequisites + +You may need to install the following packages with +your favorite package manager: +* [Apache Ant](https://ant.apache.org/) - needed for OpenCV Java support +* [SQLite](https://www.sqlite.org/index.html) - needed for lidar-optimize + +## Build the packages + +Each subdirectory has a build script which takes a single argument, +which is the installation location for the compiled libraries and +executables. + +The `buildAll.bash` script will build and install the libraries and executables +to the desired location. For example, this would install everything in a +directory named for your system architecture (e.g. 3rd-party/Darwin_x86_64 on +an intel macOS system): + +``` +../buildAll.bash 3rd-party/$(uname -s)_$(uname -m) +``` + diff --git a/support-libraries/altwg/CMakeLists.txt b/support-libraries/altwg/CMakeLists.txt new file mode 100644 index 0000000..e19e65f --- /dev/null +++ b/support-libraries/altwg/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.17) + +PROJECT(SBMT) + +find_package(SQLite3 REQUIRED) + +# built from install-altwg.sh +# find_package(GSL REQUIRED) + +set(GSL_HOME $ENV{GSL_HOME}) +set(VTK_DIR $ENV{VTK_HOME}/lib/cmake/vtk-9.0/) +find_package(VTK COMPONENTS vtkFiltersCore vtkIOLegacy vtkIOGeometry NO_MODULE) + +set(SPICE_DIR $ENV{SPICE_HOME} CACHE PATH "Path to Spice") + +add_definitions(-Wall) + +# Print all environment variables +#get_cmake_property(_variableNames VARIABLES) +#list (SORT _variableNames) +#foreach (_variableName ${_variableNames}) +# message(STATUS "${_variableName}=${${_variableName}}") +#endforeach() + +include_directories( +${GSL_HOME}/include +${SPICE_DIR}/include +${CMAKE_SOURCE_DIR} +${CMAKE_SOURCE_DIR}/gravity +${CMAKE_SOURCE_DIR}/common +) + +add_library(lidar +gravity/gravity-werner.cpp +gravity/gravity-cheng.cpp +gravity/gravity-point.h +gravity/platemodel.cpp +common/util.h +common/mathutil.h +strip-adjustment/optimizer.cpp +strip-adjustment/lidardata.cpp +strip-adjustment/closest-point-vtk.cpp +strip-adjustment/icp-gsl.cpp +strip-adjustment/optimize-gsl.cpp +) +target_link_libraries(lidar ${GSL_HOME}/lib/libgsl.a ${GSL_HOME}/lib/libgslcblas.a ${VTK_LIBRARIES} ${SPICE_DIR}/lib/cspice.a sqlite3) + +add_executable(gravity gravity/gravity.cpp) +target_link_libraries(gravity lidar) + +add_executable(lidar-optimize strip-adjustment/lidar-optimize.cpp) +target_link_libraries(lidar-optimize lidar) diff --git a/support-libraries/altwg/common/mathutil.h b/support-libraries/altwg/common/mathutil.h new file mode 100644 index 0000000..c0e33ea --- /dev/null +++ b/support-libraries/altwg/common/mathutil.h @@ -0,0 +1,188 @@ +#ifndef MATHUTIL_H +#define MATHUTIL_H + +#include +#include "vtkTransform.h" +#include "vtkQuaternion.h" + +// The following functions are copied directly from VTK + +inline void Add(const double a[3], const double b[3], double c[3]) { + for (int i = 0; i < 3; ++i) + c[i] = a[i] + b[i]; +} + +inline void Subtract(const double a[3], const double b[3], double c[3]) { + for (int i = 0; i < 3; ++i) + c[i] = a[i] - b[i]; +} + +inline double Norm(const double x[3]) { + return sqrt( x[0] * x[0] + x[1] * x[1] + x[2] * x[2] );} + +inline double Normalize(double x[3]) +{ + double den; + if ( ( den = Norm( x ) ) != 0.0 ) + { + for (int i=0; i < 3; i++) + { + x[i] /= den; + } + } + return den; +} + +inline void Cross(const double x[3], const double y[3], double z[3]) +{ + double Zx = x[1] * y[2] - x[2] * y[1]; + double Zy = x[2] * y[0] - x[0] * y[2]; + double Zz = x[0] * y[1] - x[1] * y[0]; + z[0] = Zx; z[1] = Zy; z[2] = Zz; +} + +inline double Dot(const double x[3], const double y[3]) { + return ( x[0] * y[0] + x[1] * y[1] + x[2] * y[2] );} + +inline void Outer(const double x[3], const double y[3], double A[3][3]) { + for (int i=0; i < 3; i++) + for (int j=0; j < 3; j++) + A[i][j] = x[i] * y[j]; +} + +inline void MultiplyScalar(double a[3], double s) { + for (int i = 0; i < 3; ++i) + a[i] *= s; +} + +inline void ComputeNormalDirection(double v1[3], double v2[3], + double v3[3], double n[3]) +{ + double ax, ay, az, bx, by, bz; + + // order is important!!! maintain consistency with triangle vertex order + ax = v3[0] - v2[0]; ay = v3[1] - v2[1]; az = v3[2] - v2[2]; + bx = v1[0] - v2[0]; by = v1[1] - v2[1]; bz = v1[2] - v2[2]; + + n[0] = (ay * bz - az * by); + n[1] = (az * bx - ax * bz); + n[2] = (ax * by - ay * bx); +} + +inline void ComputeNormal(double v1[3], double v2[3], + double v3[3], double n[3]) +{ + double length; + + ComputeNormalDirection(v1, v2, v3, n); + + if ( (length = sqrt((n[0]*n[0] + n[1]*n[1] + n[2]*n[2]))) != 0.0 ) + { + n[0] /= length; + n[1] /= length; + n[2] /= length; + } +} + +inline void TriangleCenter(double p1[3], double p2[3], + double p3[3], double center[3]) +{ + center[0] = (p1[0]+p2[0]+p3[0]) / 3.0; + center[1] = (p1[1]+p2[1]+p3[1]) / 3.0; + center[2] = (p1[2]+p2[2]+p3[2]) / 3.0; +} + +inline double Distance2BetweenPoints(const double x[3], + const double y[3]) +{ + return ( ( x[0] - y[0] ) * ( x[0] - y[0] ) + + ( x[1] - y[1] ) * ( x[1] - y[1] ) + + ( x[2] - y[2] ) * ( x[2] - y[2] ) ); +} + +inline double TriangleArea(double p1[3], double p2[3], double p3[3]) +{ + double a,b,c; + a = Distance2BetweenPoints(p1,p2); + b = Distance2BetweenPoints(p2,p3); + c = Distance2BetweenPoints(p3,p1); + return (0.25* sqrt(fabs(4.0*a*c - (a-b+c)*(a-b+c)))); +} + +template +void normalizeQuaternion( + const T quaternion[4], + T normalizedQuaternion[4]) +{ + const T& q0 = quaternion[0]; + const T& q1 = quaternion[1]; + const T& q2 = quaternion[2]; + const T& q3 = quaternion[3]; + + // Copied from Rotation.java in Apache Commons Math + T inv = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + + normalizedQuaternion[0] = q0 * inv; + normalizedQuaternion[1] = q1 * inv; + normalizedQuaternion[2] = q2 * inv; + normalizedQuaternion[3] = q3 * inv; +} + +template +void applyRotationToVector( + const T1 vec[3], + const T2& q0, + const T2& q1, + const T2& q2, + const T2& q3, + T2 rotatedVector[3] + ) +{ + // Copied from Rotation.java in Apache Commons Math + const T1& x = vec[0]; + const T1& y = vec[1]; + const T1& z = vec[2]; + + T2 s = q1 * x + q2 * y + q3 * z; + rotatedVector[0] = 2.0 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x; + rotatedVector[1] = 2.0 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y; + rotatedVector[2] = 2.0 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z; +} + +template +void transformPoint( + const T1 point[3], + const T1 centerOfRotation[3], + const T2 translation[3], + const T2 quaternion[4], + T2 transformedPoint[3] + ) +{ + const T2& q0 = quaternion[0]; + const T2& q1 = quaternion[1]; + const T2& q2 = quaternion[2]; + const T2& q3 = quaternion[3]; + + T1 tmpPoint[3] = {point[0], point[1], point[2]}; + + // Translate source points to centroid + tmpPoint[0] -= centerOfRotation[0]; + tmpPoint[1] -= centerOfRotation[1]; + tmpPoint[2] -= centerOfRotation[2]; + + // Apply rotation to points + applyRotationToVector(tmpPoint, q0, q1, q2, q3, transformedPoint); + + // Translate points back + transformedPoint[0] = transformedPoint[0] + centerOfRotation[0]; + transformedPoint[1] = transformedPoint[1] + centerOfRotation[1]; + transformedPoint[2] = transformedPoint[2] + centerOfRotation[2]; + + // Apply translation + transformedPoint[0] += translation[0]; + transformedPoint[1] += translation[1]; + transformedPoint[2] += translation[2]; +} + + +#endif // MATHUTIL_H diff --git a/support-libraries/altwg/common/util.h b/support-libraries/altwg/common/util.h new file mode 100644 index 0000000..c99327d --- /dev/null +++ b/support-libraries/altwg/common/util.h @@ -0,0 +1,70 @@ +#ifndef __UTIL_H__ +#define __UTIL_H__ + +#include +#include +#include + +// The following 3 functions were adapted from +// http://stackoverflow.com/questions/479080/trim-is-not-part-of-the-standard-c-c-library?rq=1 +static const std::string whiteSpaces( " \f\n\r\t\v" ); + +// Remove initial and trailing whitespace from string. Modifies string in-place +inline void trimRight( std::string& str, + const std::string& trimChars = whiteSpaces ) +{ + std::string::size_type pos = str.find_last_not_of( trimChars ); + str.erase( pos + 1 ); +} + +inline void trimLeft( std::string& str, + const std::string& trimChars = whiteSpaces ) +{ + std::string::size_type pos = str.find_first_not_of( trimChars ); + str.erase( 0, pos ); +} + +inline void trim( std::string& str, + const std::string& trimChars = whiteSpaces ) +{ + trimRight( str, trimChars ); + trimLeft( str, trimChars ); +} + + +inline bool endsWith(const std::string& fullString, const std::string& ending) +{ + if (fullString.length() >= ending.length()) + { + return (0 == fullString.compare (fullString.length() - ending.length(), ending.length(), ending)); + } + else + { + return false; + } +} + + +// The following 2 functions were adapted from http://stackoverflow.com/questions/236129/how-to-split-a-string-in-c +inline void +split(const std::string &s, char delim, std::vector &elems) +{ + std::stringstream ss(s); + std::string item; + while (std::getline(ss, item, delim)) + { + if (item.length() > 0) + elems.push_back(item); + } +} + + +inline std::vector +split(const std::string &s, char delim = ' ') +{ + std::vector elems; + split(s, delim, elems); + return elems; +} + +#endif diff --git a/support-libraries/altwg/gravity/README-gravity.txt b/support-libraries/altwg/gravity/README-gravity.txt new file mode 100644 index 0000000..296d42a --- /dev/null +++ b/support-libraries/altwg/gravity/README-gravity.txt @@ -0,0 +1,136 @@ +------------ +Introduction +------------ +This README file describes how to build and run the gravity program +for computing the gravitational acceleration and potential of an +arbitrary triangular plate model. It implements the algorithm by +Werner and Scheeres[1] as well as the simpler, faster algorithm by +Andy Cheng[2]. + + +------------------------ +Compilation instructions +------------------------ +Use CMake to compile. See CMake documentation for further details. + + +---------------------------- +Gravity program instructions +---------------------------- +This gravity executable takes various options as well as the path to +plate a model (in typical PDS format). It computes the potential, +acceleration, and acceleration magnitude and saves the values to files +in the current directory. You can choose between the Werner and Andy +Cheng's method. You can also choose between evaluating at the center +of each plate, or evaluating at the vertices of the shape model, or +averaging the values at the vertices at each plate center. You need to +provide the density (in g/cm^3) and rotation rate (in radians/per +second). It is assumed that the vertices are expressed in kilometers +and on output the potential is expressed in J/kg and the acceleration +in m/s^2. The gravity program takes the following options: + + -d Density of shape model in g/cm^3 (default is 1) + -r Rotation rate of shape model in radians/sec (default is 0) + --werner Use the Werner algorithm for computing the gravity (this is the + default if neither --werner or --cheng option provided) + --cheng Use Andy Cheng's algorithm for computing the gravity (default is to + use Werner method if neither --werner or --cheng option provided) + --centers Evaluate gravity directly at the centers of plates (this is the default + if neither --centers or -vertices or --file option provided) + --average-vertices Evaluate gravity of each plate by averaging the gravity computed at the + 3 vertices of the plate (default is to evaluate at centers) + --vertices Evaluate gravity directly at each vertex (default is to evaluate at centers) + When using this option, you must also add the --cheng option since singularities + occur at the vertices with the Werner algorithm. + --file Evaluate gravity at points specified in file (default is to evaluate + at centers) + --ref-potential If the --file option is provided, then use this option to specify the reference + potential which is needed for calculating elevation. This option is ignored if + --file is not provided. If --file is provided but --ref-potential is not + provided then no elevation data is saved out. + --columns If --file is provided, then this options controls which columns of the file are + assumed to contain the x, y, and z coordinates of the points. By default, columns + 0, 1, and 2 are read. If you wanted, say, columns 3, 4, and 5 instead you would + include this option as for example: --columns 3,4,5. Note that they are separated + by commas (no spaces) and are zero based. If --file is not provided, then this + option is ignored. + --start-index + --end-index use these 2 options to specify a range of plates or points to process. For example if + --start-index is 1000 and --end-index is 2000, then only plates or points 1000 through + 1999 are processed. This is useful for parallelizing large shape models on + multiple machines. + --save-plate-centers If specified, the centers of all plates in the shape model are saved to an + additional file. + --suffix If specified, the suffix will be appended to all output files. This is needed when + splitting large shape models into mulitple runs so that each run will be output to + different files. + --output-folder + Path to folder in which to place output files (defualt is current directory). + +(If you run it with no arguments, this is printed out). + +So for example, to compute the gravity at all plate centers using the +Werner method for Itokawa, you would do: + +./gravity -d 1.95 -r .000143857148947075 ver64q.tab + +where -d 1.95 specifies the density, and -r .000143857148947075 +specifies the rotation rate, and ver64q.tab contains the shape +model. There is no need to specify --werner or --centers since these +are the default values. On output you should see the files +ver64q.tab-potential.txt, ver64q.tab-acceleration.txt, +ver64q.tab-acceleration-magnitude.txt in the current directory which +contain the computed potential, acceleration and acceleration +magnitude. + +To run using Andy Cheng's method instead, you would type: + +./gravity -d 1.95 -r .000143857148947075 --cheng ver64q.tab + +Note the addition of the --cheng option. + +The program also prints out timing information. + +In addition there's an option for computing the potential and +acceleration at arbitrary coordinates rather than at the vertices and +centers of the plates. To use this option, specify the --file options +followed by the path to a file containing a list of coordinates in +body fixed coordinates, with one coordinate per line. For example, the +following is a valid file: +-0.15153 0.08183 0.07523 +-0.14726 0.08251 0.07657 +-0.14288 0.08309 0.07759 +-0.13851 0.08366 0.07864 +-0.13436 0.08447 0.08038 +By default the first 3 columns of the file are assumed to contain the +x, y, and z coordinates of the points. If the data is contained in +other columns, use the --columns option to specify which columns +should be read. For example if the data is contained in the 4th, 5th, +and 6th columns, you would add this option: +--columns 3,4,5 +Note the column numbers are zero based (so 0 is the first columns). + +Note also that if the --file option is provided and you also provide a +value for the reference potential with the --ref-potential option, +then the elevation is also computed for each point and is saved out in +a separate file (In the example above, this would be +ver64q.tab-elevation.txt). Note that elevation is only evaluated this +way when you have a list of arbitrary points using the --file +option. To compute elevation and slope at all plate center, use the +elevation-slope program, described below. + +Note also to compute the reference potential for an arbitrary shape +model (which you would need to compute elevation at arbitrary points), +run the elevation-slope program described below and the reference +potential value will be printed out to standard output. + + +------------------------------------ +References +------------------------------------ +[1] Werner, R.A. and Scheeres, D.J., 1997, Exterior gravitation of a +polyhedron derived and compared with harmonic and mascon gravitation +representations of asteroid 4769 Castalia, Celestial Mechanics and +Dynamical Astronomy, Vol. 65, pp. 313-344. +[2] Cheng, A.F. et al., 2012, Efficient Calculation of Effective +Potential and Gravity on Small Bodies, ACM, 1667, p. 6447. diff --git a/support-libraries/altwg/gravity/gravity-cheng.cpp b/support-libraries/altwg/gravity/gravity-cheng.cpp new file mode 100644 index 0000000..a65425f --- /dev/null +++ b/support-libraries/altwg/gravity/gravity-cheng.cpp @@ -0,0 +1,135 @@ +#include +#include "platemodel.h" + +using namespace std; + +/* + These functions compute gravitation potential and acceleration + of a closed triangular plate model using the approximation derived by A. Cheng. + */ +struct FaceCenters +{ + double center[3]; + double normal[3]; // with length equal to twice plate area +}; + +static vector faceCenters; +static Platemodel* polyData = 0; + +Platemodel* initializeGravityCheng(const char* filename) +{ + if (polyData != 0) + delete polyData; + + polyData = new Platemodel(); + polyData->load(filename); + + int pointIds[3]; + // Compute the face data + int numFaces = polyData->getNumberOfPlates(); + faceCenters.resize(numFaces); + for (int i=0; igetPlatePoints(i, pointIds); + int p1 = pointIds[0]; + int p2 = pointIds[1]; + int p3 = pointIds[2]; + + double pt1[3]; + double pt2[3]; + double pt3[3]; + polyData->getPoint(p1, pt1); + polyData->getPoint(p2, pt2); + polyData->getPoint(p3, pt3); + + TriangleCenter(pt1, pt2, pt3, fc.center); + + + polyData->getNormal(i, fc.normal); + + double area = TriangleArea(pt1, pt2, pt3); + MultiplyScalar(fc.normal, 2.0 * area); + + faceCenters[i] = fc; + } + + return polyData; +} + +double getGravityCheng(const double fieldPoint[3], double acc[3]) +{ + double potential = 0.0; + acc[0] = 0.0; + acc[1] = 0.0; + acc[2] = 0.0; + + int pointIds[3]; + + // Compute the edge data + int numFaces = polyData->getNumberOfPlates(); + for (int i=0; igetPlatePoints(i, pointIds); + int p1 = pointIds[0]; + int p2 = pointIds[1]; + int p3 = pointIds[2]; + + double pt1[3]; + double pt2[3]; + double pt3[3]; + polyData->getPoint(p1, pt1); + polyData->getPoint(p2, pt2); + polyData->getPoint(p3, pt3); + + double _2vjik[3] = { + 2.0 * pt2[0] - pt1[0] - pt3[0], + 2.0 * pt2[1] - pt1[1] - pt3[1], + 2.0 * pt2[2] - pt1[2] - pt3[2] + }; + double _2vijk[3] = { + 2.0 * pt1[0] - pt2[0] - pt3[0], + 2.0 * pt1[1] - pt2[1] - pt3[1], + 2.0 * pt1[2] - pt2[2] - pt3[2] + }; + double _2vkij[3] = { + 2.0 * pt3[0] - pt1[0] - pt2[0], + 2.0 * pt3[1] - pt1[1] - pt2[1], + 2.0 * pt3[2] - pt1[2] - pt2[2] + }; + double factor = 3.0 / Norm(_2vjik) + 3.0 / Norm(_2vijk) + 3.0 / Norm(_2vkij); + + acc[0] -= fc.normal[0] * factor; + acc[1] -= fc.normal[1] * factor; + acc[2] -= fc.normal[2] * factor; + } + else + { + potential += x_minus_R_dot_N / mag_x_minus_R; + + acc[0] -= ( (fc.normal[0] - x_minus_R[0] * x_minus_R_dot_N / (mag_x_minus_R*mag_x_minus_R)) / mag_x_minus_R ); + acc[1] -= ( (fc.normal[1] - x_minus_R[1] * x_minus_R_dot_N / (mag_x_minus_R*mag_x_minus_R)) / mag_x_minus_R ); + acc[2] -= ( (fc.normal[2] - x_minus_R[2] * x_minus_R_dot_N / (mag_x_minus_R*mag_x_minus_R)) / mag_x_minus_R ); + } + } + + potential *= 0.25; + acc[0] *= 0.25; + acc[1] *= 0.25; + acc[2] *= 0.25; + + return potential; +} diff --git a/support-libraries/altwg/gravity/gravity-cheng.h b/support-libraries/altwg/gravity/gravity-cheng.h new file mode 100644 index 0000000..53babf9 --- /dev/null +++ b/support-libraries/altwg/gravity/gravity-cheng.h @@ -0,0 +1,10 @@ +#ifndef GRAVITY_CHENG_H +#define GRAVITY_CHENG_H + +#include "platemodel.h" + +Platemodel* initializeGravityCheng(const char* filename); +double getGravityCheng(const double fieldPoint[3], double acc[3]); + + +#endif // GRAVITY_CHENG_H diff --git a/support-libraries/altwg/gravity/gravity-point.h b/support-libraries/altwg/gravity/gravity-point.h new file mode 100644 index 0000000..29bea6f --- /dev/null +++ b/support-libraries/altwg/gravity/gravity-point.h @@ -0,0 +1,22 @@ +#ifndef GRAVITY_POINT_H +#define GRAVITY_POINT_H + +#include "mathutil.h" + +inline double getGravityPoint(const double fieldPoint[3], double acc[3]) +{ + double rhat[3] = {fieldPoint[0], fieldPoint[1], fieldPoint[2]}; + double r = Normalize(rhat); + + double r2 = fieldPoint[0]*fieldPoint[0] + fieldPoint[1]*fieldPoint[1] + fieldPoint[2]*fieldPoint[2]; + + double potential = 1.0 / r; + acc[0] = -rhat[0] / r2; + acc[1] = -rhat[1] / r2; + acc[2] = -rhat[2] / r2; + + return potential; +} + + +#endif // GRAVITY_POINT_H diff --git a/support-libraries/altwg/gravity/gravity-werner.cpp b/support-libraries/altwg/gravity/gravity-werner.cpp new file mode 100644 index 0000000..fbc7fb7 --- /dev/null +++ b/support-libraries/altwg/gravity/gravity-werner.cpp @@ -0,0 +1,357 @@ +#include +#include "mathutil.h" +#include "platemodel.h" +#include + +using namespace std; + +struct EdgeKey +{ + int p1; + int p2; +}; + +struct EdgeHash +{ + size_t + operator()(const EdgeKey& key) const + { + // This hash seems to produce efficient look ups. Not sure what + // the best hash is though. + return key.p1; + } +}; + +static bool operator==(const EdgeKey& key1, const EdgeKey& key2) +{ + return key1.p1 == key2.p1 && key1.p2 == key2.p2; +} + +struct EdgeData +{ + double E[3][3]; + double edgeLength; + int p1; + int p2; +}; + +struct FaceData +{ + double F[3][3]; + int p1; + int p2; + int p3; +}; + +struct PointData +{ + double r[3]; + double r_mag; +}; + +typedef unordered_map EdgeDataMap; + +static vector edgeData; +static vector faceData; +static Platemodel* polyData = 0; +static vector pointData; + + +static void addMatrices(double a[3][3], double b[3][3], double c[3][3]) +{ + for (int i=0; i<3; ++i) + for (int j=0; j<3; ++j) + c[i][j] = a[i][j] + b[i][j]; +} + +static void Multiply3x3(const double A[3][3], const double v[3], double u[3]) +{ + u[0] = A[0][0]*v[0] + A[0][1]*v[1] + A[0][2]*v[2]; + u[1] = A[1][0]*v[0] + A[1][1]*v[1] + A[1][2]*v[2]; + u[2] = A[2][0]*v[0] + A[2][1]*v[1] + A[2][2]*v[2]; +} + +static double Abs(double a) +{ + return (a <= 0.0) ? 0.0 - a : a; +} + +/* + // For debugging +static void printmatrix(double m[3][3]) +{ + for (int i=0; i<3; ++i) + { + for (int j=0; j<3; ++j) + cout << m[i][j] << " "; + cout << endl; + } +} + +static void printvec(const char* str, double m[3]) +{ + cout << str << " : "; + for (int j=0; j<3; ++j) + cout << m[j] << " "; + cout << endl; +} +*/ + +/* + These functions compute gravitation potential and acceleration + of a closed triangular plate model using the method of Werner as + described in Werner R. A. and D. J. Scheeres (1997) CeMDA, 65, 313-344. + */ +Platemodel* initializeGravityWerner(const char* filename) +{ + if (polyData != 0) + delete polyData; + + EdgeDataMap edgeDataMap; + + polyData = new Platemodel(); + polyData->load(filename); + + int pointIds[3]; + + int numFaces = polyData->getNumberOfPlates(); + // Compute the edge data + for (int i=0; igetPlatePoints(i, pointIds); + + double cellNormal[3]; + polyData->getNormal(i, cellNormal); + + for (int j=0; j<3; ++j) + { + int p1; + int p2; + if (j < 2) + { + p1 = pointIds[j]; + p2 = pointIds[j+1]; + } + else + { + p1 = pointIds[2]; + p2 = pointIds[0]; + } + + // Put the point with the lowest id into ed so that + // the 2 identical edges always have the same point + EdgeKey key; + if (p1 < p2) + { + key.p1 = p1; + key.p2 = p2; + } + else + { + key.p1 = p2; + key.p2 = p1; + } + + // If key not found + EdgeDataMap::iterator it = edgeDataMap.find(key); + if (it == edgeDataMap.end()) + { + EdgeData ed; + ed.E[0][0] = ed.E[0][1] = ed.E[0][2] = 0.0; + ed.E[1][0] = ed.E[1][1] = ed.E[1][2] = 0.0; + ed.E[2][0] = ed.E[2][1] = ed.E[2][2] = 0.0; + ed.edgeLength = 0.0; + ed.p1 = key.p1; + ed.p2 = key.p2; + it = edgeDataMap.insert(pair(key,ed)).first; + } + + EdgeData& ed = it->second; + + // Compute unit vector from p1 to p2 + double edgeUnitVector[3]; + double pt1[3]; + double pt2[3]; + polyData->getPoint(p1, pt1); + polyData->getPoint(p2, pt2); + Subtract(pt2, pt1, edgeUnitVector); + ed.edgeLength = Normalize(edgeUnitVector); + // Compute half of the E dyad + double edgeNormal[3]; + Cross(edgeUnitVector, cellNormal, edgeNormal); + + double E[3][3]; + Outer(cellNormal, edgeNormal, E); + + addMatrices(ed.E, E, ed.E); + } + } + + // Now convert the edgeDataMap to a vector + edgeData.resize(numFaces*3/2); + int i = 0; + EdgeDataMap::const_iterator it = edgeDataMap.begin(); + EdgeDataMap::const_iterator end = edgeDataMap.end(); + for (; it != end; ++it) + { + edgeData[i] = it->second; + ++i; + } + + + // Compute the face data + faceData.resize(numFaces); + for (int i=0; igetPlatePoints(i, pointIds); + + fd.p1 = pointIds[0]; + fd.p2 = pointIds[1]; + fd.p3 = pointIds[2]; + + // Compute the F dyad + double normal[3]; + polyData->getNormal(i, normal); + Outer(normal, normal, fd.F); + + faceData[i] = fd; + } + + return polyData; +} + +static double compute_wf(const FaceData& fd) +{ + const PointData& pd1 = pointData[fd.p1]; + const PointData& pd2 = pointData[fd.p2]; + const PointData& pd3 = pointData[fd.p3]; + + double cross[3]; + Cross(pd2.r, pd3.r, cross); + + double numerator = Dot(pd1.r, cross); + double denominator = pd1.r_mag*pd2.r_mag*pd3.r_mag + + pd1.r_mag*Dot(pd2.r,pd3.r) + + pd2.r_mag*Dot(pd3.r,pd1.r) + + pd3.r_mag*Dot(pd1.r,pd2.r); + + if (Abs(numerator) < 1e-9) + numerator = -0.0; + + return 2.0 * atan2(numerator, denominator); +} + +static double compute_Le(const EdgeData& ed) +{ + const PointData& pd1 = pointData[ed.p1]; + const PointData& pd2 = pointData[ed.p2]; + + if ( Abs(pd1.r_mag + pd2.r_mag - ed.edgeLength) < 1e-9) + { + return 0.0; + } + + return log ( (pd1.r_mag + pd2.r_mag + ed.edgeLength) / (pd1.r_mag + pd2.r_mag - ed.edgeLength) ); +} + +double getGravityWerner(const double fieldPoint[3], double acc[]) +{ + double potential = 0.0; + if (acc) + { + acc[0] = 0.0; + acc[1] = 0.0; + acc[2] = 0.0; + } + + // Cache all the vectors from field point to vertices and their magnitudes + int numPoints = polyData->getNumberOfPoints(); + pointData.resize(numPoints); + for (int i=0; igetPoint(i, pd.r); + Subtract(pd.r, fieldPoint, pd.r); + pd.r_mag = Norm(pd.r); + } + + + double Er[3]; + double rEr; + double Fr[3]; + double rFr; + + int numEdges = edgeData.size(); + for (int i=0; igetNumberOfPlates(); + for (int i=0; igetNumberOfPoints(); + pointData.resize(numPoints); + for (int i=0; igetPoint(i, pd.r); + Subtract(pd.r, fieldPoint, pd.r); + pd.r_mag = Norm(pd.r); + } + + double sum = 0.0; + int numFaces = polyData->getNumberOfPlates(); + for (int i=0; i= 2.0*M_PI; +} diff --git a/support-libraries/altwg/gravity/gravity-werner.h b/support-libraries/altwg/gravity/gravity-werner.h new file mode 100644 index 0000000..54ec127 --- /dev/null +++ b/support-libraries/altwg/gravity/gravity-werner.h @@ -0,0 +1,12 @@ +#ifndef GRAVITY_WERNER_H +#define GRAVITY_WERNER_H + +#include "mathutil.h" +#include "platemodel.h" + +Platemodel* initializeGravityWerner(const char* filename); +double getGravityWerner(const double fieldPoint[3], double acc[3]); +bool isInsidePolyhedron(const double fieldPoint[3]); + + +#endif // GRAVITY_H diff --git a/support-libraries/altwg/gravity/gravity.cpp b/support-libraries/altwg/gravity/gravity.cpp new file mode 100644 index 0000000..6b59b43 --- /dev/null +++ b/support-libraries/altwg/gravity/gravity.cpp @@ -0,0 +1,561 @@ +#include +#include +#include +#include +#include "SpiceUsr.h" +#include "gravity-werner.h" +#include "gravity-cheng.h" +#include "gravity-point.h" +#include "util.h" +#include +#include +#include +#include + +using namespace std; + +//old value for gravitational constant +//static const double g_G = 6.67384e-11 * 1.0e-9; + +//gravitational constant new value as of Aug 16, 2017. units in m^3/kgs^2 +static const double defaultG = 6.67408e-11; + +typedef enum GravityAlgorithmType +{ + WERNER, + CHENG, + POINT_SOURCE +} GravityAlgorithmType; + +typedef enum HowToEvaluateAtPlate +{ + EVALUATE_AT_CENTER, + EVALUATE_AT_VERTEX, + AVERAGE_VERTICES, + FROM_FILE +} HowToEvaluateAtPlate; + +struct GravityResult +{ + double potential; + double acc[3]; + double elevation; + bool filled; +}; + +static void saveResults(char* pltfile, + string outputFolder, + const vector& results, + bool saveElevation, + string suffix) +{ + string pltfilebasename = basename(pltfile); + + string outputPot = outputFolder + "/" + pltfilebasename + "-potential.txt" + suffix; + string outputAcc = outputFolder + "/" + pltfilebasename + "-acceleration.txt" + suffix; + + cout << "saving to potential file:" << outputPot << "\n"; + cout << "saving to acceleration file:" << outputAcc << "\n"; + + ofstream foutP(outputPot.c_str()); + if (!foutP.is_open()) + { + cerr << "Error: Unable to open file for writing" << endl; + exit(1); + } + foutP.precision(16); + + ofstream foutA(outputAcc.c_str()); + if (!foutA.is_open()) + { + cerr << "Error: Unable to open file for writing" << endl; + exit(1); + } + foutA.precision(16); + + int size = results.size(); + for (int i=0; i\n\n" + << "Where:\n" + << " Path to shape model file in OBJ or Gaskell PLT format.\n\n" + << "Options:\n" + << " -d Density of shape model in g/cm^3 (default is 1)\n" + << " -r Rotation rate of shape model in radians/sec (default is 0)\n" + << " --gravConst Change the gravitational constant to use. Units are (in gcm^3/s^2)\n" + << " (default is " << defaultG << ")\n" + << " --werner Use the Werner algorithm for computing the gravity (this is the\n" + << " default if neither --werner or --cheng option provided)\n" + << " --cheng Use Andy Cheng's algorithm for computing the gravity (default is to\n" + << " use Werner method if neither --werner or --cheng option provided)\n" + << " --centers Evaluate gravity directly at the centers of plates (this is the default\n" + << " if neither --centers or -vertices or --file option provided)\n" + << " --average-vertices Evaluate gravity of each plate by averaging the gravity computed at the\n" + << " 3 vertices of the plate (default is to evaluate at centers)\n" + << " --vertices Evaluate gravity directly at each vertex (default is to evaluate at centers)\n" + << " When using this option, you must also add the --cheng option since singularities\n" + << " occur at the vertices with the Werner algorithm.\n" + << " --file Evaluate gravity at points specified in file (default is to evaluate\n" + << " at centers)\n" + << " --ref-potential If the --file option is provided, then use this option to specify the reference\n" + << " potential (in J/kg) which is needed for calculating elevation. This option is ignored if\n" + << " --file is not provided. If --file is provided but --ref-potential is not\n" + << " provided then no elevation data is saved out.\n" + << " --columns If --file is provided, then this options controls which columns of the file are\n" + << " assumed to contain the x, y, and z coordinates of the points. By default, columns\n" + << " 0, 1, and 2 are read. If you wanted, say, columns 3, 4, and 5 instead you would\n" + << " include this option as for example: --columns 3,4,5. Note that they are separated\n" + << " by commas (no spaces) and are zero based. If --file is not provided, then this\n" + << " option is ignored.\n" + << " --start-index \n" + << " --end-index use these 2 options to specify a range of plates or points to process. For example if\n" + << " --start-index is 1000 and --end-index is 2000, then only plates or points 1000 through\n" + << " 1999 are processed. This is useful for parallelizing large shape models on\n" + << " multiple machines.\n" + << " --suffix If specified, the suffix will be appended to all output files. This is needed when\n" + << " splitting large shape models into mulitple runs so that each run will be output to\n" + << " different files.\n" + << " --output-folder \n" + << " Path to folder in which to place output files (default is current directory).\n" + << endl; + + exit(1); +} + +int main(int argc, char** argv) +{ + const int numberRequiredArgs = 1; + if (argc-1 < numberRequiredArgs) + usage(); + + double density = 1.0; + double omega = 0.0; + //double density = 2.67; // for Eros + //double density = 1.95; // for Itokawa + //double density = 3.456; // for Vesta + //double density = 2.5; // for Andy Cheng's gravity poster + //double omega = 0.0003311657616706400000;// for Eros (in radians per second) + //double omega = 0.000143857148947075; // for Itokawa (in radians per second) + //double omega = (1617.3329428 / 86400.0) * (M_PI / 180.0); // for Vesta (in radians per second) + GravityAlgorithmType gravityType = WERNER; + HowToEvaluateAtPlate howToEvalute = EVALUATE_AT_CENTER; + char* fieldpointsfile = 0; + double refPotential = 0.0; + bool refPotentialProvided = false; + int fileColumns[3] = {0, 1, 2}; + int startIndex = -1; + int endIndex = -1; + string suffix = ""; + string outputFolder = "."; + double g_G = defaultG; + + int i = 1; + for(; i tokens = split(argv[++i], ','); + if (tokens.size() != 3) + usage(); + fileColumns[0] = atoi(tokens[0].c_str()); + fileColumns[1] = atoi(tokens[1].c_str()); + fileColumns[2] = atoi(tokens[2].c_str()); + } + else if (!strcmp(argv[i], "--start-index")) + { + startIndex = atoi(argv[++i]); + } + else if (!strcmp(argv[i], "--end-index")) + { + endIndex = atoi(argv[++i]); + } + else if (!strcmp(argv[i], "--suffix")) + { + suffix = argv[++i]; + } + else if (!strcmp(argv[i], "--output-folder")) + { + outputFolder = argv[++i]; + } + else if (!strcmp(argv[i], "--gravConst")) + { + g_G = atof(argv[++i]); + } + else + { + break; + } + } + + //convert grav constant to units of meters and grams + g_G = g_G * 1.0e-9; + + cout << "gravitational constant (in gcm^3/s^2) is " << g_G << "\n"; + + cout << "writing to outputFolder:" << outputFolder << "\n"; + + // There must be numRequiredArgs arguments remaining after the options. Otherwise abort. + if (argc - i != numberRequiredArgs) + usage(); + + if ((howToEvalute == EVALUATE_AT_VERTEX || howToEvalute == AVERAGE_VERTICES) && gravityType == WERNER) + { + cout << "Warning: When evaluating at vertices, use the Cheng algorithm since\n" + << "singularities occur at vertices with the Werner algorithm. Continuing anyway." + << endl; + } + + char* pltfile = argv[i]; + + cout.setf(ios::fixed,ios::floatfield); + cout.precision(2); + clock_t t1, t2; + + t1 = clock(); + Platemodel* polyData = 0; + if (gravityType == WERNER) + polyData = initializeGravityWerner(pltfile); + else if (gravityType == CHENG) + polyData = initializeGravityCheng(pltfile); + else + abort(); + t2 = clock(); + double elapsed_time = (double)(t2 - t1) / CLOCKS_PER_SEC; + cout << "Initialization time: " << elapsed_time << " sec" << endl; + + double acc[3] = {0.0, 0.0, 0.0}; + double potential = 0.0; + + vector plateResults; + + if (howToEvalute == FROM_FILE) + { + ifstream fin(fieldpointsfile); + if (fin.is_open()) + { + t1 = clock(); + string line; + int count = 0; + int lineNumber = -1; + while (getline(fin, line)) + { + ++lineNumber; + if (startIndex >= 0 && endIndex >= 0) + { + if (lineNumber < startIndex || lineNumber >= endIndex) + continue; + } + + vector tokens = split(line); + double pt[3] = { + atof(tokens[ fileColumns[0] ].c_str()), + atof(tokens[ fileColumns[1] ].c_str()), + atof(tokens[ fileColumns[2] ].c_str()) + }; + + if (gravityType == WERNER) + potential = 1.0e6*1.0e12*g_G*density*getGravityWerner(pt, acc); + else + potential = 1.0e6*1.0e12*g_G*density*getGravityCheng(pt, acc); + + acc[0] *= 1.0e3 * 1.0e12 * g_G * density; + acc[1] *= 1.0e3 * 1.0e12 * g_G * density; + acc[2] *= 1.0e3 * 1.0e12 * g_G * density; + + // add centrifugal force + if (omega != 0.0) + { + potential -= 1.0e6 * 0.5 * omega*omega * (pt[0]*pt[0] + pt[1]*pt[1]); + acc[0] += 1.0e3 * omega*omega * pt[0]; + acc[1] += 1.0e3 * omega*omega * pt[1]; + // do nothing for z component + } + + GravityResult result; + result.potential = potential; + result.acc[0] = acc[0]; + result.acc[1] = acc[1]; + result.acc[2] = acc[2]; + if (refPotentialProvided) + result.elevation = (potential - refPotential) / Norm(acc); + plateResults.push_back(result); + + if ((count+1) % 10000 == 0) + { + t2 = clock(); + double elapsed_time = (double)(t2 - t1) / CLOCKS_PER_SEC; + cout << "Time to evaluate at " << count+1 << " points: " << elapsed_time << " sec" << endl; + } + + ++count; + } + t2 = clock(); + double elapsed_time = (double)(t2 - t1) / CLOCKS_PER_SEC; + cout << "Time to evaluate total of " << count << " points: " << elapsed_time << " sec" << endl; + } + else + { + cerr << "Error: Unable to open file '" << fieldpointsfile << "'" << endl; + exit(1); + } + } + else + { + t1 = clock(); + + vector pointResults; + int numFilled = 0; + if (howToEvalute == AVERAGE_VERTICES) + { + int numPoints = polyData->getNumberOfPoints(); + pointResults.resize(numPoints); + for (i=0; igetNumberOfPlates(); + int numPoints = polyData->getNumberOfPoints(); + + if (startIndex < 0 || endIndex < 0) + { + startIndex = 0; + if (howToEvalute == EVALUATE_AT_VERTEX) + endIndex = numPoints; + else + endIndex = numPlates; + } + + cout << "total number of indicies to process: " << (endIndex - startIndex) << endl; + int counter = 1; + for (i=startIndex; igetPlatePoints(i, idList); + + double pt1[3]; + double pt2[3]; + double pt3[3]; + polyData->getPoint(idList[0], pt1); + polyData->getPoint(idList[1], pt2); + polyData->getPoint(idList[2], pt3); + + double center[3]; + + TriangleCenter(pt1, pt2, pt3, center); + + if (gravityType == WERNER) + potential = 1.0e6*1.0e12*g_G*density*getGravityWerner(center, acc); + else + potential = 1.0e6*1.0e12*g_G*density*getGravityCheng(center, acc); + + acc[0] *= 1.0e3 * 1.0e12 * g_G * density; + acc[1] *= 1.0e3 * 1.0e12 * g_G * density; + acc[2] *= 1.0e3 * 1.0e12 * g_G * density; + + // add centrifugal force + if (omega != 0.0) + { + potential -= 1.0e6 * 0.5 * omega*omega * (center[0]*center[0] + center[1]*center[1]); + acc[0] += 1.0e3 * omega*omega * center[0]; + acc[1] += 1.0e3 * omega*omega * center[1]; + // do nothing for z component + } + + if (counter % 10000 == 0) + { + t2 = clock(); + double elapsed_time = (double)(t2 - t1) / CLOCKS_PER_SEC; + cout << "Time to evaluate " << counter << " plates: " << elapsed_time << " sec" << endl; + + } + } + else if(howToEvalute == AVERAGE_VERTICES) + { + polyData->getPlatePoints(i, idList); + + double pt[3]; + acc[0] = 0.0; + acc[1] = 0.0; + acc[2] = 0.0; + potential = 0.0; + for (int j=0; j<3; ++j) + { + int ptId = idList[j]; + GravityResult& result = pointResults[ptId]; + if (!result.filled) + { + polyData->getPoint(ptId, pt); + + if (gravityType == WERNER) + result.potential = 1.0e6*1.0e12*g_G*density*getGravityWerner(pt, result.acc); + else + result.potential = 1.0e6*1.0e12*g_G*density*getGravityCheng(pt, result.acc); + + result.acc[0] *= 1.0e3 * 1.0e12 * g_G * density; + result.acc[1] *= 1.0e3 * 1.0e12 * g_G * density; + result.acc[2] *= 1.0e3 * 1.0e12 * g_G * density; + + // add centrifugal force + if (omega != 0.0) + { + result.potential -= 1.0e6 * 0.5 * omega*omega * (pt[0]*pt[0] + pt[1]*pt[1]); + result.acc[0] += 1.0e3 * omega*omega * pt[0]; + result.acc[1] += 1.0e3 * omega*omega * pt[1]; + // do nothing for z component + } + + result.filled = true; + + if ((numFilled+1) % 10000 == 0) + { + t2 = clock(); + double elapsed_time = (double)(t2 - t1) / CLOCKS_PER_SEC; + cout << "Time to evaluate " << numFilled+1 << " vertices: " << elapsed_time << " sec" << endl; + } + ++numFilled; + } + + potential += result.potential; + acc[0] += result.acc[0]; + acc[1] += result.acc[1]; + acc[2] += result.acc[2]; + } + + potential /= 3.0; + acc[0] /= 3.0; + acc[1] /= 3.0; + acc[2] /= 3.0; + } + else if (howToEvalute == EVALUATE_AT_VERTEX) + { + double pt[3]; + polyData->getPoint(i, pt); + + if (gravityType == WERNER) + potential = 1.0e6*1.0e12*g_G*density*getGravityWerner(pt, acc); + else + potential = 1.0e6*1.0e12*g_G*density*getGravityCheng(pt, acc); + + acc[0] *= 1.0e3 * 1.0e12 * g_G * density; + acc[1] *= 1.0e3 * 1.0e12 * g_G * density; + acc[2] *= 1.0e3 * 1.0e12 * g_G * density; + + // add centrifugal force + if (omega != 0.0) + { + potential -= 1.0e6 * 0.5 * omega*omega * (pt[0]*pt[0] + pt[1]*pt[1]); + acc[0] += 1.0e3 * omega*omega * pt[0]; + acc[1] += 1.0e3 * omega*omega * pt[1]; + // do nothing for z component + } + + if (counter % 10000 == 0) + { + t2 = clock(); + double elapsed_time = (double)(t2 - t1) / CLOCKS_PER_SEC; + cout << "Time to evaluate " << counter << " vertices: " << elapsed_time << " sec" << endl; + } + } + + GravityResult result; + result.potential = potential; + result.acc[0] = acc[0]; + result.acc[1] = acc[1]; + result.acc[2] = acc[2]; + plateResults.push_back(result); + + counter++; + } + + t2 = clock(); + double elapsed_time = (double)(t2 - t1) / CLOCKS_PER_SEC; + cout << "Total loops: " << counter << endl; + cout << "Total time: " << elapsed_time << " sec" << endl; + } + + // Only save out elevation if user provided a list of points and + // also specified a reference potential. If calculating at plate + // centers only, then a separate program must be used to calculate + // elevation and slope. + bool saveElevation = refPotentialProvided && howToEvalute == FROM_FILE; + saveResults(pltfile, outputFolder, plateResults, saveElevation, suffix); + + return 0; +} diff --git a/support-libraries/altwg/gravity/platemodel.cpp b/support-libraries/altwg/gravity/platemodel.cpp new file mode 100644 index 0000000..f1cc8cb --- /dev/null +++ b/support-libraries/altwg/gravity/platemodel.cpp @@ -0,0 +1,213 @@ +#include "platemodel.h" +#include "util.h" +#include +#include +#include +#include +#include + +using namespace std; + +Platemodel::Platemodel() +{ +} + +void Platemodel::loadGaskell(std::string filename) +{ + ifstream fin(filename.c_str()); + if (fin.is_open()) + { + int numPoints = -1; + int numPlates = -1; + string line; + + getline(fin, line); + trim(line); + vector tokens = split(line); + if (tokens.size() >= 1) + numPoints = atoi(tokens[0].c_str()); + if (tokens.size() == 2) + numPlates = atoi(tokens[1].c_str()); + if (tokens.size() < 1 || tokens.size() > 2) + { + cerr << "Error: File format incorrect " << endl; + exit(1); + } + + Vertex x; + for (int i=0; i tokens = split(line); + if (tokens.size() != 1) + { + cerr << "Error: File format incorrect" << endl; + exit(1); + } + numPlates = atoi(tokens[0].c_str()); + } + + Plate p; + for (int i=0; i tokens = split(line); + if (tokens.size() != 4) + { + cerr << "Error: File format incorrect" << endl; + cerr << "line content:" << line << endl; + exit(1); + } + + type = tokens[0]; + if (type == "v" || type == "V") + { + x.point[0] = atof(tokens[1].c_str()); + x.point[1] = atof(tokens[2].c_str()); + x.point[2] = atof(tokens[3].c_str()); + vertices.push_back(x); + } + else if (type == "f" || type == "F") + { + p.cell[0] = atoi(tokens[1].c_str())-1; + p.cell[1] = atoi(tokens[2].c_str())-1; + p.cell[2] = atoi(tokens[3].c_str())-1; + + ComputeNormal( + vertices[p.cell[0]].point, + vertices[p.cell[1]].point, + vertices[p.cell[2]].point, + p.normal + ); + + plates.push_back(p); + } + else + { + cerr << "Warning: Only lines beginning with 'v' or 'f' are currently parsed" << endl; + } + lindex = lindex + 1; + } + } + else + { + cerr << "Error: Unable to open file '" << filename << "'" << endl; + exit(1); + } +} + +void Platemodel::load(std::string filename) +{ + bool isObj = false; + + // Try to guess the format. Assume OBJ if the first non blank or comment line + // begins with a v or f + { + ifstream fin(filename.c_str()); + if (fin.is_open()) + { + string line; + string type; + while (getline(fin, line)) + { + trim(line); + + if (line.length() == 0 || line[0] == '#') + continue; + + vector tokens = split(line); + if (tokens.size() == 0) + { + cerr << "Error: File format not recognized" << endl; + exit(1); + } + + + if (tokens.size() == 1 || tokens.size() == 2) + { + // If true, this is the first line of the a gaskell formatted file + // which contains either the number of vertices + // or both the number of vertices and the number of plates. + isObj = false; + break; + } + + type = tokens[0]; + if (type == "v" || type == "V" || type == "f" || type == "F") + { + // If true, this is an OBJ file + isObj = true; + break; + } + } + fin.close(); + } + } + + if (isObj) + loadOBJ(filename); + else + loadGaskell(filename); +} diff --git a/support-libraries/altwg/gravity/platemodel.h b/support-libraries/altwg/gravity/platemodel.h new file mode 100644 index 0000000..98cd2fc --- /dev/null +++ b/support-libraries/altwg/gravity/platemodel.h @@ -0,0 +1,94 @@ +#ifndef PLATEMODEL_H +#define PLATEMODEL_H + +#include +#include +#include "mathutil.h" + +/** + This is bare bones plate model class similar to VTK's vtkPolydata class + for use in programs which do not have a dependency on VTK. + */ +class Platemodel +{ + struct Vertex + { + double point[3]; + }; + + struct Plate + { + int cell[3]; + double normal[3]; + }; + +public: + Platemodel(); + + void load(std::string filename); + + void getPoint(int i, double x[3]) + { + const Vertex& pt = vertices[i]; + x[0] = pt.point[0]; + x[1] = pt.point[1]; + x[2] = pt.point[2]; + } + + void getNormal(int i, double x[3]) + { + const Plate& p = plates[i]; + x[0] = p.normal[0]; + x[1] = p.normal[1]; + x[2] = p.normal[2]; + } + + void getPlatePoints(int i, int ids[3]) + { + const Plate& p = plates[i]; + ids[0] = p.cell[0]; + ids[1] = p.cell[1]; + ids[2] = p.cell[2]; + } + + void getPlateCenter(int i, double center[3]) + { + const Plate& p = plates[i]; + TriangleCenter( + vertices[p.cell[0]].point, + vertices[p.cell[1]].point, + vertices[p.cell[2]].point, + center + ); + } + + double getPlateArea(int i) + { + const Plate& p = plates[i]; + return TriangleArea( + vertices[p.cell[0]].point, + vertices[p.cell[1]].point, + vertices[p.cell[2]].point + ); + } + + int getNumberOfPoints() + { + return vertices.size(); + } + + int getNumberOfPlates() + { + return plates.size(); + } + +private: + + void loadGaskell(std::string filename); + void loadOBJ(std::string filename); + + std::vector vertices; + std::vector plates; +}; + +#endif // PLATEMODEL_H diff --git a/support-libraries/altwg/install-altwg.sh b/support-libraries/altwg/install-altwg.sh new file mode 100755 index 0000000..091450e --- /dev/null +++ b/support-libraries/altwg/install-altwg.sh @@ -0,0 +1,95 @@ +#!/bin/bash + +# This script builds the gravity and lidar-optimize tools from the +# ALTWG distribution. + +ARCH=$(uname -s)_$(uname -m) +if [ -z $1 ]; then + echo "Usage: $0 install_dir" + echo "e.g. $0 3rd-party/${ARCH}/altwg" + exit 0 +fi + +install_dir=$1 + +# Assume VTK and SPICE are installed +base=$( + cd $(dirname $install_dir) + pwd -P +) +export VTK_HOME=${base}/vtk +if [ -d ${VTK_HOME} ]; then + echo "VTK found in ${VTK_HOME}" +else + echo "VTK should be installed in ${VTK_HOME}. Please run install-vtk.sh before this script." + exit 0 +fi + +export SPICE_HOME=${base}/spice/JNISpice +if [ -d ${SPICE_HOME} ]; then + echo "SPICE found in ${SPICE_HOME}" +else + echo "SPICE should be installed in ${SPICE_HOME}. Run the install-spice.sh script first." + exit 0 +fi + +GSL_VERSION=2.8 +export GSL_HOME=${base}/gsl +if [ ! -d $GSL_HOME ]; then + echo "Installing GSL" + if [ ! -e gsl-${GSL_VERSION}.tar.gz ]; then + curl -RO https://ftp.gnu.org/gnu/gsl/gsl-${GSL_VERSION}.tar.gz + fi + tar xfz gsl-${GSL_VERSION}.tar.gz + ( + cd gsl-${GSL_VERSION} + ./configure --prefix ${GSL_HOME} + # make install cannot find certain header files? + make -j4 + make install + ) +fi + +mkdir -p $install_dir + +if [ $? -ne 0 ]; then + echo "cannot create directory $install_dir" + exit 1 +fi + +if [ ! -w $install_dir ]; then + echo "cannot write to directory $install_dir" + exit 1 +fi + +pushd $install_dir >/dev/null +install_dir=$(pwd -P) +popd >/dev/null +echo "will install ALTWG tools to $install_dir" + +set -e + +# location of this script +DIR=$( + cd $(dirname $0) + pwd -P +) + +BUILD_DIR=buildaltwg +rm -rf $BUILD_DIR +mkdir -p $BUILD_DIR +cd $BUILD_DIR + +export VTK_DIR=${VTK_HOME} +cmake -DCMAKE_BUILD_TYPE:String=Release "-DCMAKE_CXX_FLAGS_RELEASE:String=-O2 -DNDEBUG" ${DIR} +cmake --build . --config Release + +for file in gravity lidar-optimize; do + if [ "$(uname -s)" == "Darwin" ]; then + # set @rpath for macOS executables + install_name_tool -add_rpath @executable_path/../vtk/lib $file + fi + cp -p $file $install_dir +done + +echo "installed ALTWG tools. You may remove $(pwd)." diff --git a/support-libraries/altwg/strip-adjustment/closest-point-vtk.cpp b/support-libraries/altwg/strip-adjustment/closest-point-vtk.cpp new file mode 100644 index 0000000..87e256b --- /dev/null +++ b/support-libraries/altwg/strip-adjustment/closest-point-vtk.cpp @@ -0,0 +1,109 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "lidardata.h" +#include "util.h" + +static bool usePointLocator; + +static vtkPointLocator* pointLocator = 0; +static vtkCellLocator* cellLocator = 0; +static vtkGenericCell* genericCell = 0; + +static vtkPolyData* polyData = 0; + +void initializeVtk(const char* dskfile) +{ + polyData = vtkPolyData::New(); + if (endsWith(dskfile, ".vtk")) + { + vtkPolyDataReader* smallBodyReader = vtkPolyDataReader::New(); + smallBodyReader->SetFileName(dskfile); + smallBodyReader->Update(); + + polyData->ShallowCopy(smallBodyReader->GetOutput()); + usePointLocator = true; + } + else + { + vtkOBJReader* smallBodyReader = vtkOBJReader::New(); + smallBodyReader->SetFileName(dskfile); + smallBodyReader->Update(); + + polyData->ShallowCopy(smallBodyReader->GetOutput()); + usePointLocator = false; + } + + if (usePointLocator) + { + // Initialize the point locator + pointLocator = vtkPointLocator::New(); + pointLocator->FreeSearchStructure(); + pointLocator->SetDataSet(polyData); + pointLocator->AutomaticOn(); + pointLocator->SetMaxLevel(100); + pointLocator->SetNumberOfPointsPerBucket(1); + pointLocator->BuildLocator(); + } + else + { + // Initialize the cell locator + cellLocator = vtkCellLocator::New(); + cellLocator->FreeSearchStructure(); + cellLocator->SetDataSet(polyData); + cellLocator->CacheCellBoundsOn(); + cellLocator->AutomaticOn(); + cellLocator->SetMaxLevel(100); + cellLocator->SetNumberOfCellsPerNode(1); + cellLocator->BuildLocator(); + genericCell = vtkGenericCell::New(); + } + +} + +void savePointCloudToVTK(const LidarPointCloud& pointCloud, const std::string& filename) +{ + vtkPoints* points = vtkPoints::New(); + for (unsigned int i=0; iInsertNextPoint(p.targetpos); + } + + vtkPolyData* polyData = vtkPolyData::New(); + polyData->SetPoints(points); + vtkPolyDataWriter* writer = vtkPolyDataWriter::New(); + writer->SetInputData(polyData); + writer->SetFileName(filename.c_str()); + writer->SetFileTypeToBinary(); + writer->Update(); +} + +void findClosestPointVtk(const double* origin, double* closestPoint, int* found) +{ + double point[3] = {origin[0], origin[1], origin[2]}; + vtkIdType cellId; + if (usePointLocator) + { + vtkIdType vtkId = pointLocator->FindClosestPoint(point); + polyData->GetPoint(vtkId, closestPoint); + cellId = 1; // there will always be a closest point + } + else + { + int subId; + double dist2; + cellLocator->FindClosestPoint(point, closestPoint, genericCell, cellId, subId, dist2); + } + + if (cellId >= 0) + *found = 1; + else + *found = 0; +} diff --git a/support-libraries/altwg/strip-adjustment/closest-point-vtk.h b/support-libraries/altwg/strip-adjustment/closest-point-vtk.h new file mode 100644 index 0000000..f4b1b09 --- /dev/null +++ b/support-libraries/altwg/strip-adjustment/closest-point-vtk.h @@ -0,0 +1,10 @@ +#ifndef __CLOSEST_POINT_VTK_H__ +#define __CLOSEST_POINT_VTK_H__ + +#include "lidardata.h" + +void initializeVtk(const char* dskfile); +void findClosestPointVtk(const double* origin, double* closestPoint, int* found); +void savePointCloudToVTK(const LidarPointCloud& pointCloud, const std::string& filename); + +#endif diff --git a/support-libraries/altwg/strip-adjustment/gradient.ginsh b/support-libraries/altwg/strip-adjustment/gradient.ginsh new file mode 100644 index 0000000..3278fc5 --- /dev/null +++ b/support-libraries/altwg/strip-adjustment/gradient.ginsh @@ -0,0 +1,67 @@ +# This script generates C code for the partial derivatives of the +# objective function used by the lidar point optimizer. The objective +# function is the sum of squared differences between the target points +# and the transformed source points. This script only computes the +# partial derivatives for one term of the sum. After running this +# script, the generated C code should be copied into the loop that +# computes the sum of squared difference between the points. See +# the file icp-gsl.cpp in the function 'grad' to see how this was +# done. + +p0 = q0; +p1 = q1; +p2 = q2; +p3 = q3; + +inv = 1.0 / sqrt(p0 * p0 + p1 * p1 + p2 * p2 + p3 * p3); +p0 = p0 * inv; +p1 = p1 * inv; +p2 = p2 * inv; +p3 = p3 * inv; + +s0 = s0 - cor0; +s1 = s1 - cor1; +s2 = s2 - cor2; + +a = s0; +b = s1; +c = s2; + +w = p1 * a + p2 * b + p3 * c; +s0 = 2.0 * (p0 * (a * p0 - (p2 * c - p3 * b)) + w * p1) - a; +s1 = 2.0 * (p0 * (b * p0 - (p3 * a - p1 * c)) + w * p2) - b; +s2 = 2.0 * (p0 * (c * p0 - (p1 * b - p2 * a)) + w * p3) - c; + +s0 = s0 + cor0; +s1 = s1 + cor1; +s2 = s2 + cor2; + +s0 = s0 + x * m; +s1 = s1 + y * m; +s2 = s2 + z * m; + +d2 = ( s0 - t0 )^2 + ( s1 - t1 )^2 + ( s2 - t2 )^2; + +0; +0; +print_csrc(diff(d2,x)); +0; +0; +print_csrc(diff(d2,y)); +0; +0; +print_csrc(diff(d2,z)); +0; +0; +print_csrc(diff(d2,q0)); +0; +0; +print_csrc(diff(d2,q1)); +0; +0; +print_csrc(diff(d2,q2)); +0; +0; +print_csrc(diff(d2,q3)); +0; +0; diff --git a/support-libraries/altwg/strip-adjustment/icp-gsl.cpp b/support-libraries/altwg/strip-adjustment/icp-gsl.cpp new file mode 100644 index 0000000..ae9b391 --- /dev/null +++ b/support-libraries/altwg/strip-adjustment/icp-gsl.cpp @@ -0,0 +1,326 @@ +#include +#include +#include "optimize-gsl.h" +#include "closest-point-vtk.h" +#include "lidardata.h" +#include "mathutil.h" + + +static LidarPointCloud g_source; +static LidarPointCloud g_target; +static double g_sourceCentroid[3]; +static bool g_translationOnly; +static bool g_rotationOnly; + +// The purpose of the following factor is as follows. When solving the optimization +// we want the values of the independent variables to be approximately the same size. +// Now the 4 elements of the rotation quaternion are usually less then 1. However, if +// the camera is far away from the asteroid, then the values of the spacecraft position +// may be orders of magnitude greater than the quaternion values. This difference can +// mess up the optimization. Therefore, before starting the optmization, we divide the +// position by g_MULT_FACTOR so as to make the values on the order of 1. g_MULT_FACTOR is +// set to the norma of the position. Then before each time we make use of the position, +// we multiply it by g_MULT_FACTOR to get back the correct value. This way the optimization +// sees all the independent variables as having approximately the same size. +static double g_MULT_FACTOR = 1.0; + +/* Return distance squared between points x and y */ +static double vdist2(const double x[3], const double y[3]) +{ + return ( ( x[0] - y[0] ) * ( x[0] - y[0] ) + + ( x[1] - y[1] ) * ( x[1] - y[1] ) + + ( x[2] - y[2] ) * ( x[2] - y[2] ) ); +} + +/* Add vectors v1 and v2 and store result in vout */ +static void vadd(const double v1[3], const double v2[3], double vout[3]) +{ + vout[0] = v1[0] + v2[0]; + vout[1] = v1[1] + v2[1]; + vout[2] = v1[2] + v2[2]; +} + +static void centroid(const LidarPointCloud& points, double* centroid) +{ + centroid[0] = 0.0; + centroid[1] = 0.0; + centroid[2] = 0.0; + int n = points.size(); + int i; + for (i=0; i= count*step && + distance >= endDistanceToIgnore && + distance <= (totalDistance-endDistanceToIgnore)) + { + g_source.push_back(source.at(i)); + ++count; + } + } +} + +/** + Perform ICP algorithm on source and target points, both of size + n. The optimal translation that maps the source points into target + points is calculated and placed in translation. + */ +void icpGsl(LidarPointCloud& source, double translation[3], double rotation[4], double centerOfRotation[3], int maxNumPointsToUse, double endFractionToIgnore, bool translationOnly, bool rotationOnly) +{ + /* For the ICP, we do the following: + + First compute an initial transformation between the 2 point sets + by computing the centroid of each set and translating the source + by that amount to the target + + Then iterate the following: + 1. For each point in source, find the closest point in target. + 2. Minimize the ssd between these corresponding points + */ + + g_source.clear(); + g_target.clear(); + + g_translationOnly = translationOnly; + g_rotationOnly = rotationOnly; + + samplePointSpatially(source, maxNumPointsToUse, endFractionToIgnore); + + centroid(g_source, g_sourceCentroid); + + // First 3 are translation, next 4 are rotation quaternion + double transformation[7]; + transformation[0] = 0.0; + transformation[1] = 0.0; + transformation[2] = 0.0; + transformation[3] = 1.0; + transformation[4] = 0.0; + transformation[5] = 0.0; + transformation[6] = 0.0; + + if (rotationOnly == false) + { + // The following finds all the target points using the identity transformation + findAllClosestPoints(transformation); + + Point targetCentroid; + centroid(g_target, targetCentroid.targetpos); + + // Now update the current translation to be the displacement vector between centroids of the + // source and target points + transformation[0] = targetCentroid.targetpos[0] - g_sourceCentroid[0]; + transformation[1] = targetCentroid.targetpos[1] - g_sourceCentroid[1]; + transformation[2] = targetCentroid.targetpos[2] - g_sourceCentroid[2]; + } + +#ifndef NDEBUG + printf("Beginning ICP\n"); + printf("Initial value of objective function with no translation: %f\n", + func(transformation, NULL)); +#endif + + /* Now iterate */ + + int maxIter = 5000; + int i; + for (i=0;i= prevssd) + break; + } + + translation[0] = transformation[0]; + translation[1] = transformation[1]; + translation[2] = transformation[2]; + rotation[0] = transformation[3]; + rotation[1] = transformation[4]; + rotation[2] = transformation[5]; + rotation[3] = transformation[6]; + centerOfRotation[0] = g_sourceCentroid[0]; + centerOfRotation[1] = g_sourceCentroid[1]; + centerOfRotation[2] = g_sourceCentroid[2]; + + // transform the source target points and spacecraft positions using the optimal translation and rotation + int totalNumPoints = source.size(); + for (int i=0; i +#include +#include +#include +#include +#include "SpiceUsr.h" +#include "optimizer.h" +#include "lidardata.h" +#include "closest-point-vtk.h" + + +/************************************************************************ +* This program optimizes the location of a point cloud by trying to compute the best +* translation that best aligns the point cloud with a shape model. This program +* requires 3 arguments to run in this order: +* 1. objfile - the shape model in OBJ format +* 2. outputfile - the name to be given to the file which will contain the optimal transformation to be applied to the pointt cloud. +* the original point cloud file is not modified +* 3. inputfile - path to input point cloud file to be optimized +************************************************************************/ + +void saveTransformation(double translation[3], + double rotation[4], + double centerOfRotation[3], + const LidarPointCloud& pointCloudBefore, + const LidarPointCloud& pointCloudAfter, + const std::string& startId, + const std::string& stopId, + const std::string& filename) +{ + ofstream fout(filename.c_str()); + + if (fout.is_open()) + { + double minErrorBefore, minErrorAfter; + double maxErrorBefore, maxErrorAfter; + double rmsBefore, rmsAfter; + double meanErrorBefore, meanErrorAfter; + double stdBefore, stdAfter; + computePointCloudStats(pointCloudBefore, minErrorBefore, maxErrorBefore, rmsBefore, meanErrorBefore, stdBefore); + computePointCloudStats(pointCloudAfter, minErrorAfter, maxErrorAfter, rmsAfter, meanErrorAfter, stdAfter); + + fout.precision(16); + fout << scientific + << "{\n" + << "\"translation\" : [ " << translation[0] << " , " << translation[1] << " , " << translation[2] << " ],\n" + << "\"rotation\" : [ " << rotation[0] << " , " << rotation[1] << " , " << rotation[2] << " , " << rotation[3] << " ],\n" + << "\"centerOfRotation\" : [ " << centerOfRotation[0] << " , " << centerOfRotation[1] << " , " << centerOfRotation[2] << " ],\n" + << "\"startId\" : " << startId << ",\n" + << "\"stopId\" : " << stopId << ",\n" + << "\"minErrorBefore\" : " << minErrorBefore << ",\n" + << "\"maxErrorBefore\" : " << maxErrorBefore << ",\n" + << "\"rmsBefore\" : " << rmsBefore << ",\n" + << "\"meanErrorBefore\" : " << meanErrorBefore << ",\n" + << "\"stdBefore\" : " << stdBefore << ",\n" + << "\"minErrorAfter\" : " << minErrorAfter << ",\n" + << "\"maxErrorAfter\" : " << maxErrorAfter << ",\n" + << "\"rmsAfter\" : " << rmsAfter << ",\n" + << "\"meanErrorAfter\" : " << meanErrorAfter << ",\n" + << "\"stdAfter\" : " << stdAfter << "\n" + << "}\n"; + + fout.close(); + } + else + { + cerr << "Error: Unable to open file '" << filename << "'" << endl; + exit(1); + } + +} + +static void usage() +{ + cout << "This program takes lidar data and a shape model and tries to compute the optimal\n" + << "translation and rotation that best aligns the lidar data with the shape model.\n" + << "It implements a variation of the Iterative Closest Point algorithm. The program\n" + << "supports 2 ways to provide the lidar data:\n" + << "1. as a text file\n" + << "2. an SQLite database along with the start and stop id of the lidar points to use\n\n" + << "Usage: lidar-min-icp [options] \n" + << " \n\n" + << "Where:\n" + << " \n" + << " Path to shape model in OBJ format which data are optimized to.\n" + << " \n" + << " By default this is the path to SQLite database containing lidar data. If\n" + << " the --load-from-file option is used, then this is the path to a text file\n" + << " containing the lidar data. The format of the file is as follows. Each line\n" + << " must contain 7 space separated values. The first is the UTC string of the\n" + << " the lidar shot. The next 3 are the 3D coordinates of the lidar shot in\n" + << " kilometers. The final 3 values are the 3D coordinates of the spacecraft\n" + << " position in kilometers at the time of the lidar shot.\n" + << " \n" + << " The start id of the lidar shot in the SQLite database. This option is ignored\n" + << " if the --load-from-file option is specified, but it still must be specified.\n" + << " \n" + << " The stop id of the lidar shot in the SQLite database. This option is ignored\n" + << " if the --load-from-file option is specified, but it still must be specified.\n" + << " \n" + << " Path to file generated by this program upon termination of the optimization\n" + << " which contains the optimal transformation as a 4x4 matrix that\n" + << " best aligns the lidar data with the shape model, as well as other error\n" + << " statistics.\n\n" + << "Options:\n" + << " --save \n" + << " Save out the lidar data both before and after the optimization to text files.\n" + << " This option is implied by the --load-from-file option. A SPICE leap second\n" + << " kernel must be provided in order to convert from ephemeris time to UTC.\n" + << " --max-number-control-points \n" + << " max number of control points to use when optimizing the lidar data. For\n" + << " example suppose the actual number of points is 10000 points and you set this\n" + << " flag to 500. Then when doing the strip adjustment, only 500\n" + << " of the 10000 points are used to drive the optimization. This can increase\n" + << " performance significantly without sacrificing the quality of the strip\n" + << " adjustment. The control points are spread out evenly within the lidar points.\n" + << " If the max number of control points is greater than the number of lidar\n" + << " points, then all lidar points are used as control points. A value of 0\n" + << " means use all lidar points as control points. Default value is 0.\n" + << " --translation-only\n" + << " Only estimate a translation that best aligns the points with the model,\n" + << " not a rotation. By default both a translation and rotation are estimated.\n" + << " --rotation-only\n" + << " Only estimate a rotation that best aligns the points with the model,\n" + << " not a translation. By default both a translation and rotation are estimated.\n" + << " --load-from-file \n" + << " If specified then the second required argument to this program, ,\n" + << " is assumed to refer to a text file containing the lidar points as explained\n" + << " above, rather than a an SQLite database. On output, the transformed lidar\n" + << " data are also saved out to a separate file (as if the --save option was\n" + << " specified). A SPICE leap second kernel must be provided in order to convert\n" + << " from UTC to ephemeris time.\n" + << " --end-fraction-to-ignore \n" + << " Ignore points a specified fraction away from both ends of the window.\n" + << " Value must be between 0 and 1. Default is 0.\n" + << " --precision \n" + << " Number of digits to carry in the initial point cloud positions. Default is 6.\n"; + + exit(1); +} + +int main(int argc, char** argv) +{ + bool save = false; + bool translationOnly = false; + bool rotationOnly = false; + bool loadFromFile = false; + std::string leapSecondKernel; + int maxControlPoints = 0; + double endFractionToIgnore = 0.0; + int precision = 6; + + int i = 1; + for(; i +#include +#include +#include +#include +#include + + +LidarPointCloud loadPointCloudSQLite(const string& filename, + const string& startId, + const string& stopId, + const int precision) +{ + LidarPointCloud referenceTrajectory; + double scale = pow(10, precision); + + sqlite3 *db; + sqlite3_stmt *stmt; + + /* Open database */ + int retval = sqlite3_open_v2(filename.c_str(), &db, SQLITE_OPEN_READONLY, 0); + if(retval) + { + printf("Can't open database: %s\n", sqlite3_errmsg(db)); + exit(1); + } + else + { +#ifndef NDEBUG + printf("Opened database successfully\n"); +#endif + } + + + /* Create SQL statement */ + const string& query = "SELECT et, xtarget, ytarget, ztarget, xsc, ysc, zsc from lidar where idvalid between " + startId + " and " + stopId; + retval = sqlite3_prepare_v2(db, query.c_str(), -1, &stmt, 0); +#ifndef NDEBUG + printf("Running query: %s\n", query.c_str()); +#endif + if(retval) + { + printf("Query Failed, error code: %d\n", retval); + exit(1); + } + + while(true) + { + // fetch a row's status + retval = sqlite3_step(stmt); + + if(retval == SQLITE_ROW) + { + // SQLITE_ROW means fetched a row + Point p; + + p.time = sqlite3_column_double(stmt, 0); + for (int i = 0; i < 3; i++) { + p.targetpos[i] = sqlite3_column_double(stmt, i+1); + p.targetpos[i] = round(p.targetpos[i] * scale) / scale; + + p.scpos[i] = sqlite3_column_double(stmt, i+4); + } + + referenceTrajectory.push_back(p); + } + else if(retval == SQLITE_DONE) + { + // All rows finished +#ifndef NDEBUG + printf("All rows fetched\n"); +#endif + break; + } + else + { + // Some error encountered + printf("Error encountered, error code: %d\n", retval); + exit(1); + } + } + + sqlite3_finalize(stmt); + + // Close the handle to free memory + sqlite3_close(db); + +#ifndef NDEBUG + printf("Finished loading %d points from database\n", (int)referenceTrajectory.size()); +#endif + + return referenceTrajectory; +} + +LidarPointCloud loadPointCloudFromFile(const string& filename, const int precision) +{ + LidarPointCloud referenceTrajectory; + double scale = pow(10, precision); + + ifstream fin(filename.c_str()); + + if (fin.is_open()) + { + string line; + while (getline(fin, line)) + { + Point p; + vector tokens = split(line); + + for (int i = 0; i < 3; i++) { + p.targetpos[i] = atof(tokens[i+1].c_str()); + p.targetpos[i] = round(p.targetpos[i] * scale) / scale; + + p.scpos[i] = atof(tokens[i+4].c_str()); + } + + utc2et_c(tokens[0].c_str(), &p.time); + + referenceTrajectory.push_back(p); + } + + fin.close(); + } + else + { + cerr << "Error: Unable to open file '" << filename << "'" << endl; + exit(1); + } + + return referenceTrajectory; +} + +void computePointCloudStats(const LidarPointCloud& pointCloud, double& minError, double& maxError, double& rms, double& meanError, double& std) +{ + int size = pointCloud.size(); + int found; + minError = std::numeric_limits::max(); + maxError = 0.0; + double errorSum = 0.0; + double errorSquaredSum = 0.0; + for (int i=0; i maxError) + maxError = error; + } + + rms = sqrt(errorSquaredSum / (double)size); + meanError = errorSum / (double)size; + std = sqrt(errorSquaredSum / (double)size - meanError * meanError); +} + +void savePointCloudSBMT(const LidarPointCloud& pointCloud, const std::string& filename) +{ + std::ofstream fout(filename.c_str()); + char utc[64]; + + if (fout.is_open()) + { + fout.precision(16); + for (unsigned int i=0; i +#include +#include + +using namespace std; + +struct Point +{ + double time; + double targetpos[3]; + double scpos[3]; + + void print() + { + std::cout << targetpos[0] << " " << targetpos[1] << " " << targetpos[2] << std::endl; + } +}; + +typedef std::vector LidarPointCloud; + +LidarPointCloud loadPointCloudSQLite(const string& filename, + const string& startId, + const string& stopId, + const int precision); + +LidarPointCloud loadPointCloudFromFile(const string& filename, const int precision); + +void computePointCloudStats(const LidarPointCloud& pointCloud, double& minError, double& maxError, double& rms, double& meanError, double& std); + +void savePointCloudSBMT(const LidarPointCloud& pointCloud, const std::string& filename); + + +#endif // LIDARDATA_H diff --git a/support-libraries/altwg/strip-adjustment/optimize-gsl.cpp b/support-libraries/altwg/strip-adjustment/optimize-gsl.cpp new file mode 100644 index 0000000..2a91dca --- /dev/null +++ b/support-libraries/altwg/strip-adjustment/optimize-gsl.cpp @@ -0,0 +1,167 @@ +#include + + +/*----------------------------------------------------------------------- +------------------------------------------------------------------------- + This file contains GSL solver related functions +------------------------------------------------------------------------- +-----------------------------------------------------------------------*/ + + +#define DX 0.00001 + + +struct FuncParam +{ + double (*function)(const double*, void* externalParams); + void (*gradient)(const double*, double*, void *externalParams); + void* externalParams; +}; + + +#ifndef NDEBUG +static void printCurrentValue(size_t iter, double fx, const gsl_vector* x, size_t N) +{ + printf("Iter: %ld, fx = %.12f", iter, fx); + /*size_t i; + for (i=0; ifunction(gsl_vector_const_ptr(v, 0), p->externalParams); +} + + +/************************************************************************ +* This function numerically computes the gradient of func using finite differences +************************************************************************/ +static void grad(const gsl_vector *v, + void *internalParams, + gsl_vector *df) +{ + struct FuncParam* p = (struct FuncParam*)internalParams; + if (p->gradient != 0) + { + p->gradient(gsl_vector_const_ptr(v, 0), gsl_vector_ptr(df, 0), p->externalParams); + return; + } + + double f = func(v, internalParams); + size_t N = v->size; + + size_t i; + size_t j; + for (i=0; ifunction(coef2, p->externalParams); + + gsl_vector_set(df,i, (f2 - f) / DX); + } +} + + +static void fdf(const gsl_vector *x, + void *params, + double *f, + gsl_vector *df) +{ + *f = func(x, params); + grad(x, params, df); +} + + +/* + Public function + + Perform an optimization on provided function with N independent variables. + minimizer contains the initial guess and on output contains the optimal + values of the independent function that minimizes the function. + */ +void optimizeGsl(double (*function)(const double*, void *externalParams), + void (*gradient)(const double*, double*, void *externalParams), + double* minimizer, + size_t N, + void *externalParams) +{ + struct FuncParam internalParams; + internalParams.function = function; + internalParams.gradient = gradient; + internalParams.externalParams = externalParams; + + size_t iter = 0; + int status; + + const gsl_multimin_fdfminimizer_type *T; + gsl_multimin_fdfminimizer *s; + + gsl_vector *x; + gsl_multimin_function_fdf my_func; + + my_func.n = N; + my_func.f = func; + my_func.df = grad; + my_func.fdf = fdf; + my_func.params = &internalParams; + + /* Starting point, x = */ + x = gsl_vector_alloc (N); + size_t i; + for (i=0;igradient, 1e-1); + +#ifndef NDEBUG + if (status == GSL_SUCCESS) + printf ("Minimum found at:\n"); + + printCurrentValue(iter, s->f, s->x, N); +#endif + } + while (status == GSL_CONTINUE && iter < 50); + + for (i=0;ix, i); + +#ifndef NDEBUG + printf("GSL optimization terminated with status code = %d\n", status); + printCurrentValue(iter, function(minimizer, externalParams), s->x, N); + printf("\n"); +#endif + + gsl_multimin_fdfminimizer_free (s); + gsl_vector_free (x); +} diff --git a/support-libraries/altwg/strip-adjustment/optimize-gsl.h b/support-libraries/altwg/strip-adjustment/optimize-gsl.h new file mode 100644 index 0000000..f0c560e --- /dev/null +++ b/support-libraries/altwg/strip-adjustment/optimize-gsl.h @@ -0,0 +1,15 @@ +#ifndef OPIMIZEGSL_H +#define OPIMIZEGSL_H + +#include + + +/* minimizer is both the initial guess and the final returned value */ +void optimizeGsl(double (*function)(const double*, void *externalParams), + void (*gradient)(const double*, double*, void *externalParams), + double* minimizer, + size_t N, + void *externalParams); + + +#endif // OPIMIZEGSL_H diff --git a/support-libraries/altwg/strip-adjustment/optimizer.cpp b/support-libraries/altwg/strip-adjustment/optimizer.cpp new file mode 100644 index 0000000..59e3ef6 --- /dev/null +++ b/support-libraries/altwg/strip-adjustment/optimizer.cpp @@ -0,0 +1,72 @@ +#include +#include "optimizer.h" +#include "icp-gsl.h" +#include "mathutil.h" + +void Optimizer::setPointCloud(const LidarPointCloud &pointCloud) +{ + pointCloud__ = pointCloud; + maxNumberControlPoints__ = 0; + endFractionToIgnore__ = 0.0; + translationOnly__ = false; + rotationOnly__ = false; +} + +void Optimizer::setMaxNumberControlPoints(int maxPoints) +{ + maxNumberControlPoints__ = maxPoints; +} + +void Optimizer::setEndFractionToIgnore(double endFractionToIgnore) +{ + endFractionToIgnore__ = endFractionToIgnore; +} + +void Optimizer::setTranslationOnly(bool translationOnly) +{ + translationOnly__ = translationOnly; +} + +void Optimizer::setRotationOnly(bool rotationOnly) +{ + rotationOnly__ = rotationOnly; +} + +void Optimizer::optimize() +{ +#ifndef NDEBUG + printf("Optimizing point cloud with size %d\n", (int)pointCloud__.size()); +#endif + + optimizedPointCloud__ = pointCloud__; + + if (translationOnly__) + icpGsl(optimizedPointCloud__, optimalTranslation__, optimalRotation__, optimalCenterOfRotation__, maxNumberControlPoints__, endFractionToIgnore__, true, false); + else if (rotationOnly__) + icpGsl(optimizedPointCloud__, optimalTranslation__, optimalRotation__, optimalCenterOfRotation__, maxNumberControlPoints__, endFractionToIgnore__, false, true); + else + icpGsl(optimizedPointCloud__, optimalTranslation__, optimalRotation__, optimalCenterOfRotation__, maxNumberControlPoints__, endFractionToIgnore__, false, false); + +#ifndef NDEBUG + printf("Finished optimizing point cloud\n\n"); +#endif +} + +LidarPointCloud Optimizer::getOptimizedPointCloud() const +{ + return optimizedPointCloud__; +} + +void Optimizer::getOptimalTransformation(double translation[3], double rotation[4], double centerOfRotation[3]) const +{ + translation[0] = optimalTranslation__[0]; + translation[1] = optimalTranslation__[1]; + translation[2] = optimalTranslation__[2]; + rotation[0] = optimalRotation__[0]; + rotation[1] = optimalRotation__[1]; + rotation[2] = optimalRotation__[2]; + rotation[3] = optimalRotation__[3]; + centerOfRotation[0] = optimalCenterOfRotation__[0]; + centerOfRotation[1] = optimalCenterOfRotation__[1]; + centerOfRotation[2] = optimalCenterOfRotation__[2]; +} diff --git a/support-libraries/altwg/strip-adjustment/optimizer.h b/support-libraries/altwg/strip-adjustment/optimizer.h new file mode 100644 index 0000000..0ca81d6 --- /dev/null +++ b/support-libraries/altwg/strip-adjustment/optimizer.h @@ -0,0 +1,51 @@ +#ifndef OPTIMIZER_H +#define OPTIMIZER_H + +#include "lidardata.h" + +/** + * Class for finding the optimal translation of a lidar point cloud that best aligns it with + * a shape model. To use this class, set the point cloud by calling setPointCloud. Then call optimize(). + * Finally, you can get the optimized (translated) point cloud by calling getOptimizedPointCloud() or the + * optimal translation by calling getOptimalTranslation. + */ +class Optimizer +{ +public: + + void setPointCloud(const LidarPointCloud& pointCloud); + + void setMaxNumberControlPoints(int maxPoints); + + void setEndFractionToIgnore(double endFractionToIgnore); + + void setTranslationOnly(bool translationOnly); + + void setRotationOnly(bool rotationOnly); + + void optimize(); + + LidarPointCloud getOptimizedPointCloud() const; + + void getOptimalTransformation(double translation[3], double rotation[4], double centerOfRotation[3]) const; + +private: + + LidarPointCloud pointCloud__; + + int maxNumberControlPoints__; + + double endFractionToIgnore__; + + bool translationOnly__; + + bool rotationOnly__; + + LidarPointCloud optimizedPointCloud__; + + double optimalTranslation__[3]; + double optimalRotation__[4]; + double optimalCenterOfRotation__[3]; +}; + +#endif diff --git a/support-libraries/buildAll.bash b/support-libraries/buildAll.bash new file mode 100755 index 0000000..f956a6e --- /dev/null +++ b/support-libraries/buildAll.bash @@ -0,0 +1,86 @@ +#!/bin/bash + +ARCH=$(uname -s)_$(uname -m) +if [ -z $1 ]; then + echo "Usage: $0 install_dir" + echo "e.g. $0 3rd-party/"'$(uname -s)_$(uname -m)' + echo -e "\nThis would build everything in 3rd-party/$(uname -s)_$(uname -m) on this machine." + exit 0 +fi + +install_dir=$1 +mkdir -p $install_dir + +if [ $? -ne 0 ]; then + echo "cannot create directory $install_dir" + exit 1 +fi + +if [ ! -w $install_dir ]; then + echo "cannot write to directory $install_dir" + exit 1 +fi + +pushd $install_dir >/dev/null +install_dir=$(pwd -P) +popd >/dev/null +echo "will install to $install_dir" + +set -e + +# location of this script +DIR=$( + cd $(dirname $0) + pwd -P +) + +build_dir=build +mkdir -p $build_dir +if [ $? -ne 0 ]; then + echo "cannot create directory $build_dir" + exit 1 +fi + +if [ ! -w $build_dir ]; then + echo "cannot write to directory $build_dir" + exit 1 +fi + +cd $build_dir + +VTK_INSTALL=${install_dir}/vtk +if [ -d ${VTK_INSTALL} ]; then + echo "${VTK_INSTALL} exists. If you want to build VTK, remove this directory and call this script again." +else + ${DIR}/vtk/install-vtk.sh $VTK_INSTALL +fi + +SPICE_INSTALL=${install_dir}/spice +if [ -d ${SPICE_INSTALL} ]; then + echo "${SPICE_INSTALL} exists. If you want to build SPICE, remove this directory and call this script again." +else + ${DIR}/spice/install-spice.sh $SPICE_INSTALL +fi + +ALTWG_INSTALL=${install_dir}/altwg +if [ -d ${ALTWG_INSTALL} ]; then + echo "${ALTWG_INSTALL} exists. If you want to build the ALTWG tools, remove this directory and call this script again." +else + ${DIR}/altwg/install-altwg.sh $ALTWG_INSTALL +fi + +echo -n "looking for ant " +if hash ant 2>/dev/null; then + ant=$(which ant) + echo "... found at $ant." + OPENCV_INSTALL=${install_dir}/opencv + if [ -d ${OPENCV_INSTALL} ]; then + echo "${OPENCV_INSTALL} exists. If you want to build OpenCV, remove this directory and call this script again." + else + ${DIR}/opencv/install-opencv.sh $OPENCV_INSTALL + fi +else + echo "... not found. Skipping OpenCV compilation." +fi + +${DIR}/util/createInfoFiles/install-createInfoFiles.sh ${install_dir} diff --git a/support-libraries/mkPackage.bash b/support-libraries/mkPackage.bash new file mode 100755 index 0000000..91e38d6 --- /dev/null +++ b/support-libraries/mkPackage.bash @@ -0,0 +1,10 @@ +#!/bin/bash + +root=SBCLT_support-$(date -u +"%Y.%m.%d") +mkdir -p dist/${root} +rsync -a README.md altwg buildAll.bash spice vtk dist/${root} +cd dist +tar cfz ${root}.tar.gz ./${root} +/bin/rm -fr ./${root} + +echo -e "Created ./dist/${root}.tar.gz" diff --git a/support-libraries/opencv/install-opencv.sh b/support-libraries/opencv/install-opencv.sh new file mode 100755 index 0000000..9494925 --- /dev/null +++ b/support-libraries/opencv/install-opencv.sh @@ -0,0 +1,51 @@ +#!/bin/bash + +if [ -z $1 ]; then + ARCH=$(uname -s)_$(uname -m) + echo "Usage: $0 install_dir" + echo "e.g. $0 3rd-party/${ARCH}/opencv" + exit 0 +fi + +TAG=4.7.0 + +install_dir=$1 +mkdir -p $install_dir +install_dir=$(cd "$install_dir"; pwd -P) + +if [ $? -ne 0 ]; then + echo "cannot create directory $install_dir" + exit 1 +fi + +if [ ! -w $install_dir ]; then + echo "cannot write to directory $install_dir" + exit 1 +fi +echo "will install OpenCV to $install_dir" + +set -e + +BUILD_DIR=buildopencv +rm -rf $BUILD_DIR +mkdir -p $BUILD_DIR +cd $BUILD_DIR + +curl -L -o opencv-${TAG}.zip https://github.com/opencv/opencv/archive/refs/tags/${TAG}.zip +curl -L -o opencv_contrib-${TAG}.zip https://github.com/opencv/opencv_contrib/archive/refs/tags/${TAG}.zip + +unzip -q opencv-${TAG}.zip +unzip -q opencv_contrib-${TAG}.zip + +mkdir build +cd build + +VTK_DIR=$(dirname $install_dir)/vtk/lib/cmake/vtk-9.2 +if [ ! -d ${VTK_DIR} ]; then + unset VTK_DIR +fi + +cmake -DCMAKE_BUILD_TYPE=Release -DVTK_DIR=${VTK_DIR} -DCMAKE_INSTALL_PREFIX=$install_dir -DOPENCV_EXTRA_MODULES_PATH=../opencv_contrib-${TAG}/modules -DOPENCV_ENABLE_NONFREE=ON -DBUILD_ZLIB=OFF ../opencv-${TAG} + +make -j4 +make install diff --git a/support-libraries/spice/install-spice.sh b/support-libraries/spice/install-spice.sh new file mode 100755 index 0000000..78ef8d8 --- /dev/null +++ b/support-libraries/spice/install-spice.sh @@ -0,0 +1,102 @@ +#!/bin/bash + +ARCH=$(uname -s)_$(uname -m) +if [ -z $1 ]; then + echo "Usage: $0 install_dir" + echo "e.g. $0 3rd-party/${ARCH}/spice" + exit 0 +fi + +install_dir=$1 +mkdir -p $install_dir + +if [ $? -ne 0 ]; then + echo "cannot create directory $install_dir" + exit 1 +fi + +if [ ! -w $install_dir ]; then + echo "cannot write to directory $install_dir" + exit 1 +fi + +pushd $install_dir >/dev/null +install_dir=$(pwd -P) +popd >/dev/null +echo "will install to $install_dir" + +set -e + +# location of this script +DIR=$( + cd $(dirname $0) + pwd -P +) + +SRCDIR=${DIR}/src/${ARCH} + +# Install the JNISpice library + +if [ ! -e ${SRCDIR}/JNISpice.tar.Z ]; then + mkdir -p ${SRCDIR} + pushd ${SRCDIR} >/dev/null + if [ "$ARCH" == "Darwin_x86_64" ]; then + curl -RO https://naif.jpl.nasa.gov/pub/naif/misc/JNISpice/MacIntel_OSX_AppleC_Java1.8_64bit/packages/JNISpice.tar.Z + elif [ "$ARCH" == "Darwin_arm64" ]; then + if [ -e /project/spice/toolkit/latest/JNISpice.tar.Z ]; then + # preinstalled on srn-devmac1 + cp -p /project/spice/toolkit/latest/JNISpice.tar.Z . + else + echo "NAIF SPICE package not found for $ARCH" + fi + elif [ "$ARCH" == "Linux_x86_64" ]; then + curl -RO -N https://naif.jpl.nasa.gov/pub/naif/misc/JNISpice/PC_Linux_GCC_Java1.8_64bit/packages/JNISpice.tar.Z + else + echo "NAIF SPICE package not found for $ARCH" + exit 0 + fi + popd >/dev/null +fi + +# Get the FORTRAN package + +if [ ! -e ${SRCDIR}/toolkit.tar.Z ]; then + mkdir -p ${SRCDIR} + pushd ${SRCDIR} >/dev/null + if [ "$ARCH" == "Darwin_x86_64" ]; then + curl -RO https://naif.jpl.nasa.gov/pub/naif/toolkit/FORTRAN/MacIntel_OSX_gfortran_64bit/packages/toolkit.tar.Z + elif [ "$ARCH" == "Darwin_arm64" ]; then + if [ -e /project/spice/toolkit/latest/toolkit.tar.Z ]; then + # preinstalled on srn-devmac1 + cp -p /project/spice/toolkit/latest/toolkit.tar.Z . + else + echo "NAIF SPICE package not found for $ARCH" + fi + elif [ "$ARCH" == "Linux_x86_64" ]; then + curl -RO -N https://naif.jpl.nasa.gov/pub/naif/toolkit//FORTRAN/PC_Linux_gfortran_64bit/packages/toolkit.tar.Z + else + echo "NAIF SPICE package not found for $ARCH" + exit 0 + fi + popd >/dev/null +fi + +cd $install_dir + +if [ -e ${SRCDIR}/toolkit.tar.Z ]; then + rm -rf toolkit + tar xf ${SRCDIR}/toolkit.tar.Z +else + echo "${SRCDIR}/toolkit.tar.Z does not exist" +fi + +if [ -e ${SRCDIR}/JNISpice.tar.Z ]; then + rm -rf JNISpice + tar xf ${SRCDIR}/JNISpice.tar.Z + ( + cd JNISpice/src/JNISpice + jar cfv ${install_dir}/spice.jar spice + ) +else + echo "${SRCDIR}/JNISpice.tar.Z does not exist" +fi diff --git a/support-libraries/util/.gitkeep b/support-libraries/util/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/support-libraries/util/createInfoFiles/Makefile b/support-libraries/util/createInfoFiles/Makefile new file mode 100644 index 0000000..02bbd60 --- /dev/null +++ b/support-libraries/util/createInfoFiles/Makefile @@ -0,0 +1,74 @@ +# SPICE +SPICE_DIR = /project/sbmtpipeline/software/spice/cspice-20220921/cspice +SPICE_LIB = $(SPICE_DIR)/lib/cspice.a +# +# CFITSIO +#CFITSIO = cfitsio +#CFITSIO_LIBS = -L./cfitsio -lcfitsio +#LIB_CFITSIO = cfitsio/libcfitsio.a +CFITSIO = +CFITSIO_LIBS = +LIB_CFITSIO = +# +# Compiler +CXX = g++ +# Change -O2 to -O0 -g for debugging +COPT = -O2 +CFLAGS = -Wall --pedantic -Wno-long-long +CINCLUDES = -I$(SPICE_DIR)/include -I. +CLIBS = $(SPICE_LIB) +# +# Main tool (createInfoFiles). +TOOL = createInfoFiles +SRC = createInfoFiles.cpp getSpacecraftState.cpp getTargetState.cpp getFov.cpp saveInfoFile.cpp +OBJ = $(SRC:.cpp=.o) + +# SPICE pointing tool (computePointing). +SPICE_TOOL = computePointing +SPICE_SRC = computePointing.cpp getSpacecraftState.cpp getTargetState.cpp getFov.cpp saveInfoFile.cpp +SPICE_OBJ = $(SPICE_SRC:.cpp=.o) + +$(TOOL): $(OBJ) + $(CXX) -o $@ $(COPT) $(CFLAGS) $(CINCLUDES) $(OBJ) $(CLIBS) + +$(SPICE_TOOL): $(CFITSIO) $(SPICE_OBJ) + $(CXX) -o $@ $(COPT) $(CFLAGS) $(CINCLUDES) $(SPICE_OBJ) $(CLIBS) + +all: $(CFITSIO) $(TOOL) $(SPICE_TOOL) + +cfitsio: $(LIB_CFITSIO) + +cfitsio/libcfitsio.a: + @echo "Configuring cfitsio prior to building..." + @(cd cfitsio; ./configure > configure.txt 2>&1; \ + if test $$? -ne 0; then \ + echo "Errors occurred; see file cfitsio/configure.txt for more info." >&2; \ + exit 1; \ + else \ + echo "done"; \ + fi) + @echo "Building cfitsio..." + @(cd cfitsio; make libcfitsio.a >& make.txt 2>&1 ; \ + if test $$? -ne 0; then \ + echo "Errors occurred; see file cfitsio/make.txt for more info." >&2; \ + exit 1; \ + else \ + echo "done"; \ + fi) + +clean: + rm -f *.o + @(if test -f cfitsio/Makefile; then cd cfitsio; make clean > /dev/null 2>&1; fi) + +distclean: + rm -f *.o + rm -f $(TOOL) $(SPICE_TOOL) + rm -rf *.dSYM/ + @(if test -f cfitsio/Makefile; then cd cfitsio; make distclean > /dev/null 2>&1; fi) + +.SUFFIXES: + +.SUFFIXES: .cpp .o + +.cpp.o: + $(CXX) -c -o $@ $(COPT) $(CFLAGS) $(CINCLUDES) $< diff --git a/support-libraries/util/createInfoFiles/computePointing.cpp b/support-libraries/util/createInfoFiles/computePointing.cpp new file mode 100644 index 0000000..4ec1db8 --- /dev/null +++ b/support-libraries/util/createInfoFiles/computePointing.cpp @@ -0,0 +1,470 @@ +#include "computePointing.hpp" + +#include "SpiceUsr.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +const char * const g_programName = "computeInstrumentPointing"; +int g_status = 0; + +enum CLParId { + BODY, + FRAME, + HELP, +// IMAGE, + INSTRUMENT, + MK, + OUT_FILE, + QUIET, + SC_ID, + TIME, + NUMBER_PARS +}; + +const char * const CLParams[] = { + "b", + "f", + "h", +// "img", + "i", + "m", + "o", + "q", + "s", + "t" +}; + +SpiceChar ERROR_REPORT[] = "SHORT"; +SpiceChar RETURN[] = "RETURN"; + +} + +namespace spice_pointing { + +class InvalidCommandLine : public std::runtime_error { +public: + InvalidCommandLine(const std::string & what): std::runtime_error(what) {} + + InvalidCommandLine(const InvalidCommandLine & source): std::runtime_error(source) {} +}; + +struct CommandLinePar { + CommandLinePar(const std::string & key): m_key(key), m_value(), m_isDefined(false), m_isKey(false) {} + + CommandLinePar(const std::string & key, const std::string & value, bool isValueKeywordName): + m_key(key), m_value(value), m_isDefined(true), m_isKey(isValueKeywordName) {} + + CommandLinePar(const CommandLinePar & source): m_key(source.m_key), m_value(source.m_value), + m_isDefined(source.m_isDefined), m_isKey(source.m_isKey) {} + + CommandLinePar & operator=(const CommandLinePar & source) { + m_key = source.m_key; + m_value = source.m_value; + m_isDefined = source.m_isDefined; + m_isKey = source.m_isKey; + + return *this; + } + + std::string m_key; + std::string m_value; + bool m_isDefined; + bool m_isKey; +}; + +class CommandLineParameters { +public: + typedef std::map MapType; + + CommandLineParameters(const MapType * pars): m_pars(pars) {} + + virtual ~CommandLineParameters() { + if (m_pars != 0) { + for (MapType::const_reverse_iterator itor = m_pars->rbegin(); itor != m_pars->rend(); ++itor) { + delete itor->second; + } + delete m_pars; + } + } + + const CommandLinePar& operator[](CLParId parId) const { + return *m_pars->at(parId); + } + +private: + const MapType * const m_pars; +}; + +CommandLineParameters * interpretCommandLine(int argc, char ** argv); +void computeSpicePointing(const CommandLineParameters & pars); +void usage(std::ostream & ios); +SpiceDouble parseET(const std::string & time); +bool isSpiceError(); + +// namespace spice_pointing +//////////////////////////////////////////////////////////////////////////////// +} + +std::ostream & operator<<(std::ostream & os, const spice_pointing::CommandLinePar & par) { + os << "Par \"" << par.m_key << "\" " + << (par.m_isDefined ? "= \"" + par.m_value + "\"" : "(undefined)"); + return os; +} + +namespace { + +std::string trim(const std::string & s) { + std::string::const_iterator begin(s.begin()); + std::string::const_iterator end(s.end()); + + while (begin != end && std::isspace(*begin)) { + ++begin; + } + + while (begin != end && std::isspace(*end)) { + --end; + } + + return std::string(begin, end); +} + +void reportMissingPar(const spice_pointing::CommandLinePar & par, std::stringstream & ss, std::string & delim) { + if (!par.m_isDefined) { + ss << delim << "-" << par.m_key; +// if (par.m_key != CLParams[HELP] && {par.m_key != CLParams[IMAGE]) { +// ss << " (or -k" << par.m_key << ")"; +// } + delim = ", "; + } +} + +bool extractPar(spice_pointing::CommandLinePar & par, const std::string & arg) { + spice_pointing::CommandLinePar newPar(par); + + bool foundMatch = false; + + std::string flag("-" + par.m_key + "="); + std::string::size_type flagSize(flag.size()); + + // Look for -par=value. + if (arg.compare(0, flagSize, flag) == 0) { + newPar.m_value = arg.substr(flagSize); + newPar.m_isDefined = true; + foundMatch = true; +// } else if (par.m_key != CLParams[HELP] && par.m_key != CLParams[IMAGE]) { +// // Don't accept keyword option for either help or image file name inputs. +// +// // Look for -kpar=value (keyword). +// flag = "-k" + par.m_key + "="; +// flagSize = flag.size(); +// +// if (arg.compare(0, flagSize, flag) == 0) { +// newPar.m_value = arg.substr(flagSize); +// newPar.m_isDefined = true; +// newPar.m_isKey = true; +// foundMatch = true; +// } + } + + if (!foundMatch) { + // Look for -par (boolean parameters, e.g., -h for help/usage). + flag = "-" + par.m_key; + if (arg.compare(flag) == 0) { + newPar.m_value = "true"; + newPar.m_isDefined = true; + foundMatch = true; + } + } + + if (foundMatch) { + if (par.m_isDefined) { + // Check for duplicate parameters. + std::stringstream ss; + ss << "duplicated parameter? Attempted to parse parameter \"" + arg + "\" into already-defined parameter " << par; + throw spice_pointing::InvalidCommandLine(ss.str()); + } + + par = newPar; + } + + return foundMatch; +} + +void extractPar(spice_pointing::CommandLinePar & par, int argc, char ** argv) { + for (int i = 1; i < argc; ++i) { + extractPar(par, argv[i]); + } +} + +bool isTrue(const spice_pointing::CommandLinePar & par) { + using namespace std; + + if (par.m_isDefined) { + string parString(par.m_value); + + transform(parString.begin(), parString.end(), parString.begin(), ::tolower); + + return parString == "t" || parString == "true"; + } + + return false; +} + +// namespace +//////////////////////////////////////////////////////////////////////////////// +} + +namespace spice_pointing { + +CommandLineParameters * interpretCommandLine(int argc, char ** argv) { + using namespace std; + + for (int i = 0; i < argc; ++i) { + } + + CommandLineParameters::MapType * pars = new CommandLineParameters::MapType(); + + bool isKey = false; + for (int i = 0; i < NUMBER_PARS; ++i) { + const CLParId id = static_cast(i); + + const string & key(CLParams[i]); + CommandLinePar * par = new CommandLinePar(key); + +// extractPar(*par, argc, argv); + + isKey |= par->m_isKey; + + pars->insert(make_pair(id, par)); + } + + for (char ** arg = argv + 1; arg < argv + argc; ++arg) { + bool foundMatch = false; + for (int i = 0; i < NUMBER_PARS; ++i) { + const CLParId id = static_cast(i); + + if (extractPar(*pars->at(id), *arg)) { + foundMatch = true; + // Do NOT break out of loop here so extractPar may find any duplicated parameters. + } + } + + if (!foundMatch) { + throw InvalidCommandLine(string("no match for command line parameter \"") + *arg + "\""); + } + } + + if (!isTrue(*pars->at(HELP))) { + // Validate all parameters prior to run if user did not request showing usage. + stringstream ss; + string delim; + +// const CommandLinePar * imageFile = pars->at(IMAGE); +// +// if (isKey && !imageFile->m_isDefined) { +// reportMissingPar(*imageFile, ss, delim); +// ss << " (image file required if using any -k options)"; +// } + + reportMissingPar(*pars->at(BODY), ss, delim); + reportMissingPar(*pars->at(FRAME), ss, delim); + reportMissingPar(*pars->at(INSTRUMENT), ss, delim); + reportMissingPar(*pars->at(MK), ss, delim); + reportMissingPar(*pars->at(SC_ID), ss, delim); + reportMissingPar(*pars->at(TIME), ss, delim); + + string missing = ss.str(); + if (!missing.empty()) { + throw InvalidCommandLine("missing required parameter(s): " + missing); + } + } + + return new CommandLineParameters(pars); +} + +void computeSpicePointing(const CommandLineParameters & pars) { + using namespace std; + + const char * body(pars[BODY].m_value.c_str()); + const char * frame(pars[FRAME].m_value.c_str()); + const char * instr(pars[INSTRUMENT].m_value.c_str()); + const char * mkFile(pars[MK].m_value.c_str()); + const char * scId(pars[SC_ID].m_value.c_str()); + const char * time(pars[TIME].m_value.c_str()); + + furnsh_c(mkFile); + if (isSpiceError()) { + stringstream ss; + ss << "unable to initialize SPICE environment using metakernel file \"" << mkFile << "\""; + throw runtime_error(ss.str()); + } + + double et = parseET(time); + if (isSpiceError()) { + stringstream ss; + ss << "unable to parse time \"" << time << "\""; + throw runtime_error(ss.str()); + } + + double scPosition[3]; + double unused[3]; + double boredir[3]; + double updir[3]; + double frustum[12]; + double sunPosition[3]; + + getSpacecraftState(et, scId, body, frame, scPosition, unused); + if (isSpiceError()) { + stringstream ss; + ss << "unable to get position of spacecraft " << scId << " in the " << frame << " frame"; + throw runtime_error(ss.str()); + } + + getTargetState(et, scId, body, frame, "SUN", sunPosition, unused); + if (isSpiceError()) { + stringstream ss; + ss << "unable to get the sun position in the " << frame << " frame"; + throw runtime_error(ss.str()); + } + + getFov(et, scId, body, frame, instr, boredir, updir, frustum); + if (isSpiceError()) { + stringstream ss; + ss << "unable to get the " << instr << " FOV in the " << frame << " frame"; + throw runtime_error(ss.str()); + } + + const CommandLinePar & outFile = pars[OUT_FILE]; + if (outFile.m_isDefined) { + const char * outFileString(outFile.m_value.c_str()); + saveInfoFile(outFileString, time, scPosition, boredir, updir, frustum, sunPosition); + } + + if (!isTrue(pars[QUIET])) { + writeInfo(cout, time, scPosition, boredir, updir, frustum, sunPosition); + } +} + +void usage(std::ostream & ios) { + std::stringstream ss; + + ss + << "--------------------------------------------------------------------------------" + << "\nTool: " << g_programName + << "\n--------------------------------------------------------------------------------" + << "\nDescription: from a collection of SPICE kernels specified using a single SPICE" + << "\n metakernel file, compute SPICE pointing and FOV information for an instrument" + << "\n on a spacecraft with respect to a body in a particular frame at a particular" + << "\n moment of time. Returns 0 for success and non-0 if a problem occurs setting" + << "\n up SPICE, loading kernels, or computing the SPICE pointing information. The" + << "\n computed information may be displayed and/or written to an output INFO file." + << "\n" + << "\n This tool may be used simply to test whether a given set of inputs would" + << "\n return a valid SPICE pointing. To do this, do not specify an output file (no" + << "\n -o parameter), use -q to suppress the output if desired, and check the" + << "\n tool's exit value." + << "\n--------------------------------------------------------------------------------" + << "\nUsage: " << g_programName << " " + << "\n where are:" + << "\n" + << "\n -m= (required), is the full path to the" + << "\n metakernel file used to load all SPICE kernels" + << "\n -s= (required), is the name of" + << "\n the spacecraft as referenced in the SPICE kernels" + << "\n -i= (required), is the name of the" + << "\n instrument as referenced in the SPICE kernels" + << "\n -b= (required), is the name of the body used by SPICE as the" + << "\n origin from which to compute the pointing" + << "\n -f= (required), is the name of the SPICE frame in which to" + << "\n compute the SPICE pointing" + << "\n -t=