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}/

rviz_simulator/README.md

@@ -0,0 +1,69 @@
+# `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 Kame.  
+
+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 `rviz_simulator/config/photoneo_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 with the name `"detections_<ROS_timestamp>"` in the `rviz_simulator` package folder.  
+E.g. `detections_1565880269`:
+```
+world_T_camera:
+  rotation: [3.1404926443667587, -6.1629514791161225e-08, -0.083128525595253286]
+  translation: [-3.4547343254089355, 1.3038516098695761e-07, 2.4355044364929199]
+detections:
+  - targetID: 3
+    size: [0.10000000000000001, 0.10000000000000001]
+    corners:
+      0: [749.04850065520918, 426.08777643692048]
+      1: [795.65300431471246, 426.13839052125752]
+      2: [795.65300129230377, 380.00886350617435]
+      3: [749.04849785406611, 380.05948125315223]
+  - targetID: 4
+    size: [0.10000000000000001, 0.10000000000000001]
+    corners:
+      0: [294.01203861603608, 425.59359128189027]
+      1: [338.63436644541946, 425.64205265550714]
+      2: [338.63436555204686, 380.50523728899429]
+      3: [294.0120379256731, 380.55370216948273]
+```
+
+
+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/initialize_simulator.yaml

@@ -0,0 +1,14 @@
+world_frame_id: "ROSWorld"
+
+# target parameters
+num_targets: 5
+target_size: [.1, 0.1, 0.01]
+distance_between_targets: 1.0
+starting_target_position: [0.0, 0.0, 0.0]                   # geometery_msgs::Point
+starting_target_orientation: [-1, 0, 0, 0, 0, 1, 0, 1, 0]   # Eigen::Matrix3d ColMajor
+
+# camera parameters
+camera_intrinsics_file: "photoneo_camera_intrinsics.yaml" 
+starting_camera_position: [0, 3, 0 ]                        # geometery_msgs::Point
+starting_camera_orientation: [-1, 0, 0, 0, 0, -1, 0, -1, 0] # Eigen::Matrix3d ColMajor
+

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"
+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]