diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h
index c07cd8188ef01b4cff273fdba81ded028f61db8f..b1d4892d55f7af80d349cc0065c23abea4efc3b0 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/include/hpp/fcl/BVH/BVH_model.h
@@ -107,12 +107,14 @@ public:
   /// @brief Access the bv giving the its index
   const BVNode<BV>& getBV(int id) const
   {
+    assert (id < num_bvs);
     return bvs[id];
   }
 
   /// @brief Access the bv giving the its index
   BVNode<BV>& getBV(int id)
   {
+    assert (id < num_bvs);
     return bvs[id];
   }
 
diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h
index 88ed5d23949ef53f4c8a54375fa72c6f069d589d..fc61c366c14190cb31cc2d664d56a647fa918d29 100644
--- a/include/hpp/fcl/collision_data.h
+++ b/include/hpp/fcl/collision_data.h
@@ -259,7 +259,7 @@ public:
   }
 
   /// @brief get all the contacts
-  void getContacts(std::vector<Contact>& contacts_)
+  void getContacts(std::vector<Contact>& contacts_) const
   {
     contacts_.resize(contacts.size());
     std::copy(contacts.begin(), contacts.end(), contacts_.begin());