Camera class added and fiducial marker detections

rviz_simulator/CMakeLists.txt

@@ -1,3 +1,8 @@
+# CERES
+# https://groups.google.com/forum/#!topic/ceres-solver/wt8lf1aXrfg
+# https://github.com/catkin/catkin_simple
+# https://github.com/ethz-asl/ceres_catkin
+
 cmake_minimum_required(VERSION 2.8.3)
 project(rviz_simulator)
 set (CMAKE_CXX_STANDARD 11)
@@ -11,11 +16,20 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   visualization_msgs
   interactive_markers
-
+  eigen_conversions
+  tf_conversions
+  tf
+  roslib
 )
 
 ## System dependencies are found with CMake's conventions
 find_package(Boost REQUIRED COMPONENTS system)
+find_package(Ceres REQUIRED)
+# find_package(Eigen REQUIRED)
+# find_package(cmake_modules REQUIRED)
+
+# find_package(catkin_simple REQUIRED)
+# catkin_simple()
 
 
 ## Uncomment this if the package has a setup.py. This macro ensures
@@ -106,7 +120,7 @@ find_package(Boost REQUIRED COMPONENTS system)
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES rviz_simulator
-  CATKIN_DEPENDS roscpp std_msgs visualization_msgs interactive_markers
+  CATKIN_DEPENDS roscpp std_msgs visualization_msgs interactive_markers roslib
   # DEPENDS system_lib
 )
 
@@ -119,6 +133,8 @@ catkin_package(
 include_directories(include  
   ${catkin_INCLUDE_DIRS} 
   ${boost_INCLUDE_DIRS}
+  ${Eigen_INCLUDE_DIRS}
+  ${CERES_INCLUDE_DIRS}
 )
 
 
@@ -150,6 +166,7 @@ include_directories(include
  ## target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 
 
+
 #############
 ## Testing ##
 #############
@@ -165,22 +182,51 @@ include_directories(include
 ## add_executable(one_marker src/one_marker.cpp)
 ## target_link_libraries(one_marker ${catkin_LIBRARIES})
 
+FIND_PACKAGE(yaml-cpp REQUIRED)
+
+
+# simulate
 add_executable(simulate 
   src/simulate.cpp  
 )
 
-## Declare a C++ library
-add_library(${PROJECT_NAME}
-  src/simulate.cpp
+add_library(simulate_lib
+  src/camera.cpp
   src/target.cpp
 )
 
-target_link_libraries(simulate 
+## Declare a C++ library
+#add_library(${PROJECT_NAME}
+#  src/simulate.cpp
+#  src/camera.cpp
+#  src/target.cpp
+#)
+
+target_link_libraries(simulate
+  simulate_lib
   ${catkin_LIBRARIES} 
-  ${PROJECT_NAME} 
   ${boost_LIBRARIES}
+  ${CERES_LIBRARIES}
+  yaml-cpp
 )
 
+#target_link_libraries(simulate 
+#  ${PROJECT_NAME} yaml-cpp
+#)
+
+# synthetic_optimization
+# add_executable(synthetic_optimization src/synthetic_optimization.cpp)
+#target_link_libraries(synthetic_optimization
+ # ${catkin_LIBRARIES} 
+ # ${boost_LIBRARIES}
+ # ${CERES_LIBRARIES}
+ # yaml-cpp
+#)
+
+# target_link_libraries(${PROJECT_NAME} yaml-cpp)
+
+
+
 # add_dependencies()
 
 #############
@@ -198,11 +244,11 @@ target_link_libraries(simulate
 # )
 
 ## Mark executables and/or libraries for installation
- install(TARGETS ${PROJECT_NAME} 
-   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
+# install(TARGETS ${PROJECT_NAME} 
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
 
 ## Mark cpp header files for installation
  install(DIRECTORY include/${PROJECT_NAME}/
@@ -216,4 +262,4 @@ target_link_libraries(simulate
 #   # myfile1
 #   # myfile2
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
+# )

rviz_simulator/README.md

@@ -0,0 +1,45 @@
+# `rviz_simulator`
+
+
+## Package Description
+The `rviz_simulator` is an interactive 3D environment built using RViz and Interactive Markers that can be used to generate fiducial target pixel locations.  
+
+
+## Installation Instructions
+Requires ROS Kinectic Kane.  
+
+Clone the repo into the `src` folder of your catkin workspace.
+Run `catkin_make` in the root of your workspace to build the `rviz_simulator` package.  
+
+### Required Dependencies
+- YAML-cpp [Installation Instructions](https://github.com/jbeder/yaml-cpp)
+- Eigen [Installation Instructions](http://eigen.tuxfamily.org/dox/GettingStarted.html)
+- Ceres [Installation Instructions](http://ceres-solver.org/installation.html)
+
+## Execution Instructions
+Edit the `initialize_simulator.yaml` file to change the simulator configuration.
+
+Naviate to the root of your catkin workspace and run the `simulate.launch` file with the following command:  
+> `roslaunch rviz_simulator simulate.launch`
+
+`simulate.launch` launches 2 nodes in normal mode:
+- `rviz`
+- `simulate`
+
+and 4 nodes in `debug_mode`:
+- `rviz`
+- `rqt_console`
+- `rqt_logger_level`
+- `simulate`
+
+To enter `debug_mode`, run the launch file with the command line arg 
+> `debug_mode:=true`
+
+By default, the `realsense_camera_intrinsics.yaml` file is loaded. To specify another camera intrinsics file, run the launch file with the command line arg
+> `camera_intrinsics_file:=<camera_intrinsics_file_name.yaml>`  
+
+The `simulator` node creates a new folder `"detections_ROS_timestamp"` in the `rviz_simulator` package folder. A new detections folder with a new timestamp is created each time `roslaunch` is run.
+
+Drag around the virtual camera and multiple virtual fiducial targets.  
+
+Left click on the camera to generate a `YAML` file, in the created detections folder, with the measured target locations.  

rviz_simulator/config/david_camera.yaml

@@ -0,0 +1,20 @@
+name: "David's Camera (Camera)"
+
+width: 640
+height: 480
+fx: 630.346767
+fy: 624.904413
+cx: 314.108329
+cy: 241.591565
+near_clip: 0.1
+far_clip: 3.0
+
+# distortions
+k1: 0.096908
+k2: -0.143400
+k3: 0.0
+k4: 0.0
+k5: 0.0
+k6: 0.0
+p1: -0.002487
+p2: 0.001367

rviz_simulator/config/initialize_simulator.yaml

@@ -0,0 +1,21 @@
+world_frame_id: "ROSWorld"
+
+# target parameters
+
+# colors to be decoded as std_msgs::ColorRGBA 
+# stored in yaml as [red, green, blue, alpha]
+grey: [0.5, 0.5, 0.5, 1.0]
+blue: [0.0, 0.67, 0.9, 1.0]
+orange: [1.0, 0.39, 0.12, 1.0]
+
+num_targets_in_line: 5
+distance_between_targets: 1.0
+starting_target_position: [0.0, 0.0, 0.0]   # geometery_msgs::Point
+starting_target_orientation: [0, 0, 0, 1]   # geometry_msgs::Quaternion
+target_scale: 0.1
+
+# camera parameters
+camera_intrinsics_file: "realsense_camera_intrinsics.yaml" 
+starting_camera_positon: [0, 3, 0 ]   # geometery_msgs::Point
+starting_camera_orientation: [0, -0.7071068, 0.7071068, 0]  # geometry_msgs::Quaternion (roll 90, yaw 180)
+camera_scale: 0.2

rviz_simulator/config/photoneo_camera_intrinsics.yaml

@@ -0,0 +1,20 @@
+image_width: 1032
+image_height: 772
+camera_name: "Photoneo (Camera)"
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [1109.596, 0, 503.712, 0, 1109.596, 403.0737, 0, 0, 1]
+distortion_model: "plumb_bob"
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [0, 0, 0, 0, 0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [468.456, 0, 320, 0, 0, 468.456, 240, 0, 0, 0, 1, 0]

rviz_simulator/config/realsense_camera_intrinsics.yaml

@@ -0,0 +1,22 @@
+image_width: 1280
+image_height: 720
+camera_name: "Realsense Camera (Camera)"
+near_clip: 0.1      # remove this
+far_clip: 5         # remove this
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [468.456, 0, 320, 0, 468.456, 240, 0, 0, 1]
+distortion_model: "plumb_bob"
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [0, 0, 0, 0, 0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [468.456, 0, 320, 0, 0, 468.456, 240, 0, 0, 0, 1, 0]