diff --git a/libviso2/CMakeLists.txt b/libviso2/CMakeLists.txt
index 51d5fc4..d766b41 100644
--- a/libviso2/CMakeLists.txt
+++ b/libviso2/CMakeLists.txt
@@ -4,13 +4,17 @@ project(libviso2)
 find_package(catkin REQUIRED )
 
 option(ARM_CROSS_COMPILATION "ARM Cross Compilation" OFF)
+option(ARM64_CROSS_COMPILATION "ARM64 Cross Compilation" OFF)
 
 if(ARM_CROSS_COMPILATION)
   SET(CMAKE_SYSTEM_PROCESSOR arm)
   SET(CMAKE_CXX_FLAGS -mfpu=neon)
-else(ARM_CROSS_COMPILATION)
+elseif (ARM64_CROSS_COMPILATION)
+  SET(CMAKE_SYSTEM_PROCESSOR aarch64)
+  add_definitions(-D__ARM_NEON__)
+else()
   add_definitions(-msse3)
-endif(ARM_CROSS_COMPILATION)
+endif()
 
 catkin_package(
   INCLUDE_DIRS libviso2/src
diff --git a/libviso2/libviso2/CMakeLists.txt b/libviso2/libviso2/CMakeLists.txt
index 862b27a..d8560f8 100644
--- a/libviso2/libviso2/CMakeLists.txt
+++ b/libviso2/libviso2/CMakeLists.txt
@@ -1,28 +1,30 @@
-# project
-cmake_minimum_required (VERSION 2.6)
-project (libviso2)
-
-# directories
-set (LIBVISO2_SRC_DIR src)
-
-# include directory
-include_directories("${LIBVISO2_SRC_DIR}")
-
-# use sse3 instruction set
-if(ARM_CROSS_COMPILATION)
-  SET(CMAKE_SYSTEM_PROCESSOR arm)
-  SET(CMAKE_CXX_FLAGS -mfpu=neon)
-else(ARM_CROSS_COMPILATION)
-  add_definitions(-msse3)
-endif
-
-# sources
-FILE(GLOB LIBVISO2_SRC_FILES "src/*.cpp")
-
-# make release version
-set(CMAKE_BUILD_TYPE Release)
-
-# demo program
-add_executable(viso2 ${LIBVISO2_SRC_FILES})
-target_link_libraries (viso2 png)
-
+# project
+cmake_minimum_required (VERSION 2.6)
+project (libviso2)
+
+# directories
+set (LIBVISO2_SRC_DIR src)
+
+# include directory
+include_directories("${LIBVISO2_SRC_DIR}")
+
+if(ARM_CROSS_COMPILATION)
+  SET(CMAKE_SYSTEM_PROCESSOR arm)
+  SET(CMAKE_CXX_FLAGS -mfpu=neon)
+elseif (ARM64_CROSS_COMPILATION)
+  SET(CMAKE_SYSTEM_PROCESSOR aarch64)
+  add_definitions(-D__ARM_NEON__)
+else()
+  add_definitions(-msse3)
+endif()
+
+# sources
+FILE(GLOB LIBVISO2_SRC_FILES "src/*.cpp")
+
+# make release version
+set(CMAKE_BUILD_TYPE Release)
+
+# demo program
+add_executable(viso2 ${LIBVISO2_SRC_FILES})
+target_link_libraries (viso2 png)
+
diff --git a/libviso2/libviso2/src/triangle.cpp b/libviso2/libviso2/src/triangle.cpp
index 8de1557..cfce936 100644
--- a/libviso2/libviso2/src/triangle.cpp
+++ b/libviso2/libviso2/src/triangle.cpp
@@ -5950,19 +5950,27 @@ void mergehulls(struct mesh *m, struct behavior *b, struct otri *farleft,
 /*                                                                           */
 /*****************************************************************************/
 
-void divconqrecurse(struct mesh *m, struct behavior *b, vertex *sortarray,
+int divconqrecurse(struct mesh *m, struct behavior *b, vertex *sortarray,
                     int vertices, int axis,
                     struct otri *farleft, struct otri *farright)
 {
   struct otri midtri, tri1, tri2, tri3;
   struct otri innerleft, innerright;
   float area;
-  int divider;
+  int divider, result;
+  result = 0;
 
   if (b->verbose > 2) {
     printf("  Triangulating %d vertices.\n", vertices);
   }
-  if (vertices == 2) {
+  if(vertices <= 1){
+    //This shouldn't happen, but it can and if not handled, we will have infinite recursion and crash.*/
+    //What we'll do here to try and avoid crashing is just to make no triangles...
+    if (b->verbose > 2) {
+            printf("  Invalid divconqrecurse requested, can't have less than 2 vertices (Requested %d).\n", vertices);
+        }
+        result = 1; //Invalid.
+    } else if (vertices == 2) {
     /* The triangulation of two vertices is an edge.  An edge is */
     /*   represented by two bounding triangles.                  */
     maketriangle(m, b, farleft);
@@ -5988,7 +5996,6 @@ void divconqrecurse(struct mesh *m, struct behavior *b, vertex *sortarray,
     }
     /* Ensure that the origin of `farleft' is sortarray[0]. */
     lprev(*farright, *farleft);
-    return;
   } else if (vertices == 3) {
     /* The triangulation of three vertices is either a triangle (with */
     /*   three bounding triangles) or two edges (with four bounding   */
@@ -6085,13 +6092,12 @@ void divconqrecurse(struct mesh *m, struct behavior *b, vertex *sortarray,
       printf("  Creating ");
       printtriangle(m, b, &tri3);
     }
-    return;
   } else {
     /* Split the vertices in half. */
     divider = vertices >> 1;
     /* Recursively triangulate each half. */
-    divconqrecurse(m, b, sortarray, divider, 1 - axis, farleft, &innerleft);
-    divconqrecurse(m, b, &sortarray[divider], vertices - divider, 1 - axis,
+    result += divconqrecurse(m, b, sortarray, divider, 1 - axis, farleft, &innerleft);
+    result += divconqrecurse(m, b, &sortarray[divider], vertices - divider, 1 - axis,
                    &innerright, farright);
     if (b->verbose > 1) {
       printf("  Joining triangulations with %d and %d vertices.\n", divider,
@@ -6100,6 +6106,7 @@ void divconqrecurse(struct mesh *m, struct behavior *b, vertex *sortarray,
     /* Merge the two triangulations into one. */
     mergehulls(m, b, farleft, &innerleft, &innerright, farright, axis);
   }
+  return result;
 }
 
 long removeghosts(struct mesh *m, struct behavior *b, struct otri *startghost)
@@ -6210,7 +6217,11 @@ long divconqdelaunay(struct mesh *m, struct behavior *b)
   }
 
   /* Form the Delaunay triangulation. */
-  divconqrecurse(m, b, sortarray, i, 0, &hullleft, &hullright);
+  if(divconqrecurse(m, b, sortarray, i, 0, &hullleft, &hullright)){
+    /* Will only return a non-zero value if something went wrong. */
+    trifree((int *) sortarray);
+    return 0l;
+  }
   trifree((int *) sortarray);
 
   return removeghosts(m, b, &hullleft);
diff --git a/viso2_ros/CMakeLists.txt b/viso2_ros/CMakeLists.txt
index bae26ee..57b7694 100644
--- a/viso2_ros/CMakeLists.txt
+++ b/viso2_ros/CMakeLists.txt
@@ -20,6 +20,7 @@ find_package(OpenCV REQUIRED)
 find_package(PCL REQUIRED)
 
 option(ARM_CROSS_COMPILATION "ARM Cross Compilation" OFF)
+option(ARM64_CROSS_COMPILATION "ARM64 Cross Compilation" OFF)
 
 add_message_files( DIRECTORY msg FILES VisoInfo.msg)
 
@@ -33,9 +34,12 @@ catkin_package()
 if(ARM_CROSS_COMPILATION)
   SET(CMAKE_SYSTEM_PROCESSOR arm)
   SET(CMAKE_CXX_FLAGS -mfpu=neon)
-else(ARM_CROSS_COMPILATION)
+elseif (ARM64_CROSS_COMPILATION)
+  SET(CMAKE_SYSTEM_PROCESSOR aarch64)
+  add_definitions(-D__ARM_NEON__)
+else()
   add_definitions(-msse3)
-endif(ARM_CROSS_COMPILATION)
+endif()
 
 include_directories(src ${libviso2_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})