diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4e2d1027edfccd7f999e0f350121bb481347f118..33b6335a344d944fd2b64d1853c62c12e36ede37 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -76,7 +76,7 @@ search_for_boost()
 # Optional dependencies
 add_optional_dependency("octomap >= 1.6")
 if (OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS)
-  include_directories(${OCTOMAP_INCLUDE_DIRS})
+  include_directories(SYSTEM ${OCTOMAP_INCLUDE_DIRS})
   link_directories(${OCTOMAP_LIBRARY_DIRS})
   SET(HPP_FCL_HAVE_OCTOMAP TRUE)
   add_definitions (-DHPP_FCL_HAVE_OCTOMAP)
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index 96a0896b4f6c349500b325c3834ccbe6b0872400..1c0d831e5f1d4da24f9bedf54e6a0521d5eae283 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -130,7 +130,7 @@ namespace fcl
             col = true;
             details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
             details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
-            assert (epa_status != details::EPA::Failed);
+            assert (epa_status != details::EPA::Failed); (void) epa_status;
             Vec3f w0 (Vec3f::Zero());
             for(size_t i = 0; i < epa.result.rank; ++i)
               {
@@ -162,6 +162,7 @@ namespace fcl
           break;
         default:
           assert (false && "should not reach type part.");
+          return true;
         }
       return col;
     }
@@ -173,7 +174,9 @@ namespace fcl
                          FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
                          Vec3f& normal) const
     {
+#ifndef NDEBUG
       FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+#endif
       bool compute_normal (true);
       Vec3f guess(1, 0, 0);
       if(enable_cached_guess) guess = cached_guess;
diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp
index e3617b731de03ee5203a1ddeb6477b5010065ae8..58ab55200a01cdbc29b3fc03ff554a8264ca1316 100644
--- a/src/BVH/BVH_model.cpp
+++ b/src/BVH/BVH_model.cpp
@@ -712,10 +712,8 @@ int BVHModel<BV>::recursiveBuildTree(int bv_id, int first_primitive, int num_pri
         const Vec3f& p1 = vertices[t[0]];
         const Vec3f& p2 = vertices[t[1]];
         const Vec3f& p3 = vertices[t[2]];
-        FCL_REAL x = (p1[0] + p2[0] + p3[0]) / 3.0;
-        FCL_REAL y = (p1[1] + p2[1] + p3[1]) / 3.0;
-        FCL_REAL z = (p1[2] + p2[2] + p3[2]) / 3.0;
-        p << x, y, z;
+
+        p = (p1 + p2 + p3) / 3.;
       }
       else
       {
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 7a7a6fba92c6456fe8385bde4c08a6047053b7d7..91b4116748d6b67472cb69629281188309040bf8 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -155,8 +155,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
     if (result.numContacts () < request.num_max_contacts) {
       Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
       const Vec3f& p1 = distanceResult.nearest_points [0];
-      const Vec3f& p2 = distanceResult.nearest_points [1];
-      assert (p1 == p2);
+      assert (p1 == distanceResult.nearest_points [1]);
       contact.pos = p1;
       contact.normal = distanceResult.normal;
       contact.penetration_depth = -distance;