1 files changed, 46 insertions, 0 deletions
diff --git a/internal/ceres/array_utils.cc b/internal/ceres/array_utils.cc
index 673baa4..205ddaf 100644
--- a/internal/ceres/array_utils.cc
+++ b/internal/ceres/array_utils.cc
@@ -30,9 +30,13 @@
 
 #include "ceres/array_utils.h"
 
+#include <algorithm>
 #include <cmath>
 #include <cstddef>
+#include <string>
+#include <vector>
 #include "ceres/fpclassify.h"
+#include "ceres/stringprintf.h"
 
 namespace ceres {
 namespace internal {
@@ -55,6 +59,20 @@ bool IsArrayValid(const int size, const double* x) {
   return true;
 }
 
+int FindInvalidValue(const int size, const double* x) {
+  if (x == NULL) {
+    return size;
+  }
+
+  for (int i = 0; i < size; ++i) {
+    if (!IsFinite(x[i]) || (x[i] == kImpossibleValue))  {
+      return i;
+    }
+  }
+
+  return size;
+};
+
 void InvalidateArray(const int size, double* x) {
   if (x != NULL) {
     for (int i = 0; i < size; ++i) {
@@ -63,5 +81,33 @@ void InvalidateArray(const int size, double* x) {
   }
 }
 
+void AppendArrayToString(const int size, const double* x, string* result) {
+  for (int i = 0; i < size; ++i) {
+    if (x == NULL) {
+      StringAppendF(result, "Not Computed  ");
+    } else {
+      if (x[i] == kImpossibleValue) {
+        StringAppendF(result, "Uninitialized ");
+      } else {
+        StringAppendF(result, "%12g ", x[i]);
+      }
+    }
+  }
+}
+
+void MapValuesToContiguousRange(const int size, int* array) {
+  std::vector<int> unique_values(array, array + size);
+  std::sort(unique_values.begin(), unique_values.end());
+  unique_values.erase(std::unique(unique_values.begin(),
+                                  unique_values.end()),
+                      unique_values.end());
+
+  for (int i = 0; i < size; ++i) {
+    array[i] = std::lower_bound(unique_values.begin(),
+                                unique_values.end(),
+                                array[i]) - unique_values.begin();
+  }
+}
+
 }  // namespace internal
 }  // namespace ceres