diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3ad1df1..2c8cae8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,8 +1,7 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.12)
 project(msckf_vio)
 
-
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+add_compile_options(-std=c++11 -Wall)
 
 # Modify cmake module path if new .cmake files are required
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake")
@@ -18,11 +17,13 @@ find_package(catkin REQUIRED COMPONENTS
   tf_conversions
   random_numbers
   message_generation
+  nodelet
   image_transport
   cv_bridge
   message_filters
   pcl_conversions
   pcl_ros
+  std_srvs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -32,17 +33,15 @@ find_package(OpenCV REQUIRED)
 find_package(Cholmod REQUIRED)
 find_package(SPQR REQUIRED)
 
+##################
+## ROS messages ##
+##################
 add_message_files(
   FILES
 
   FeatureMeasurement.msg
   CameraMeasurement.msg
   TrackingInfo.msg
-
-  #DebugImuState.msg
-  #DebugCamState.msg
-  #DebugState.msg
-  #DebugMsckfInfo.msg
 )
 
 generate_messages(
@@ -50,18 +49,24 @@ generate_messages(
   std_msgs
 )
 
+###################################
+## catkin specific configuration ##
+###################################
 catkin_package(
   INCLUDE_DIRS include
-#  LIBRARIES msckf_vio
+  LIBRARIES msckf_vio image_processor
   CATKIN_DEPENDS
     roscpp std_msgs tf nav_msgs sensor_msgs geometry_msgs
-    eigen_conversions tf_conversions message_runtime
+    eigen_conversions tf_conversions random_numbers message_runtime
     image_transport cv_bridge message_filters pcl_conversions
     pcl_ros std_srvs
-  DEPENDS
-    Boost EIGEN3 OpenCV
+  DEPENDS Boost EIGEN3 OpenCV CHOLMOD SPQR
 )
 
+###########
+## Build ##
+###########
+
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
@@ -72,10 +77,6 @@ include_directories(
   ${SPQR_INCLUDES}
 )
 
-link_directories(
-  ${catkin_LIBRARY_DIRS}
-)
-
 # Msckf Vio
 add_library(msckf_vio
   src/msckf_vio.cpp
@@ -129,19 +130,45 @@ target_link_libraries(image_processor_nodelet
   ${catkin_LIBRARIES}
 )
 
-# Feature initialization test
-catkin_add_gtest(test_feature_init
-  test/feature_initialization_test.cpp
-)
-add_dependencies(test_feature_init
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-)
-target_link_libraries(test_feature_init
-  ${catkin_LIBRARIES}
+#############
+## Install ##
+#############
+
+install(TARGETS
+  msckf_vio msckf_vio_nodelet image_processor image_processor_nodelet
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-# Math utils test
-catkin_add_gtest(test_math_utils
-  test/math_utils_test.cpp
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  PATTERN "*_nodelet.h" EXCLUDE
 )
+
+install(FILES nodelets.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+if(CATKIN_ENABLE_TESTING)
+  # Feature initialization test
+  catkin_add_gtest(test_feature_init
+    test/feature_initialization_test.cpp
+  )
+  add_dependencies(test_feature_init
+    ${${PROJECT_NAME}_EXPORTED_TARGETS}
+    ${catkin_EXPORTED_TARGETS}
+  )
+  target_link_libraries(test_feature_init
+    ${catkin_LIBRARIES}
+  )
+
+  # Math utils test
+  catkin_add_gtest(test_math_utils
+    test/math_utils_test.cpp
+  )
+endif()
diff --git a/package.xml b/package.xml
index 9200202..66d6b26 100644
--- a/package.xml
+++ b/package.xml
@@ -22,8 +22,6 @@
   <depend>eigen_conversions</depend>
   <depend>tf_conversions</depend>
   <depend>random_numbers</depend>
-  <depend>message_generation</depend>
-  <depend>message_runtime</depend>
   <depend>nodelet</depend>
   <depend>image_transport</depend>
   <depend>cv_bridge</depend>
@@ -31,11 +29,15 @@
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
   <depend>std_srvs</depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
 
   <depend>libpcl-all-dev</depend>
   <depend>libpcl-all</depend>
   <depend>suitesparse</depend>
 
+  <test_depend>rosunit</test_depend>
+
   <export>
     <nodelet plugin="${prefix}/nodelets.xml"/>
   </export>