initial commit

CMakeLists.txt

@@ -0,0 +1,204 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(calibrate_mocap_and_camera)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  ar_sys
+  camera_calibration
+  gencpp
+  eigen_conversions
+    tf2 
+    tf2_ros
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Boost REQUIRED)
+find_package(ecl_eigen REQUIRED)
+find_package(OpenCV REQUIRED)
+if (OpenCV_VERSION VERSION_EQUAL "3")
+  add_definitions("-DOPENCV3=1")
+endif()
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+#  CATKIN_DEPENDS ar_sys camera_calibration gencpp
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(optitrack_to_camera
+#   src/${PROJECT_NAME}/optitrack_to_camera.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(optitrack_to_camera ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(optitrack_to_camera_node src/optitrack_to_camera_node.cpp)
+include_directories(include
+        SYSTEM ${BOOST_INCLUDE_DIRS}
+               ${catkin_INCLUDE_DIRS}
+               ${EIGEN_INCLUDE_DIRS}
+)
+
+add_executable(${PROJECT_NAME} src/calibrate_mocap_and_camera.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBS})
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(optitrack_to_camera_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(optitrack_to_camera_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS optitrack_to_camera optitrack_to_camera_node
+#   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}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_optitrack_to_camera.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

data/single/pose_calib_00.yml

@@ -0,0 +1,52 @@
+%YAML:1.0
+aruco_bc_nmarkers: 24
+aruco_bc_mInfoType: 0
+aruco_bc_markers:
+   - { id:532, corners:[ [ -215., -325., 0. ], [ -115., -325., 0. ], [
+       -115., -225., 0. ], [ -215., -225., 0. ] ] }
+   - { id:716, corners:[ [ -105., -325., 0. ], [ -5., -325., 0. ], [ -5.,
+       -225., 0. ], [ -105., -225., 0. ] ] }
+   - { id:442, corners:[ [ 5., -325., 0. ], [ 105., -325., 0. ], [ 105.,
+       -225., 0. ], [ 5., -225., 0. ] ] }
+   - { id:474, corners:[ [ 115., -325., 0. ], [ 215., -325., 0. ], [
+       215., -225., 0. ], [ 115., -225., 0. ] ] }
+   - { id:392, corners:[ [ -215., -215., 0. ], [ -115., -215., 0. ], [
+       -115., -115., 0. ], [ -215., -115., 0. ] ] }
+   - { id:277, corners:[ [ -105., -215., 0. ], [ -5., -215., 0. ], [ -5.,
+       -115., 0. ], [ -105., -115., 0. ] ] }
+   - { id:177, corners:[ [ 5., -215., 0. ], [ 105., -215., 0. ], [ 105.,
+       -115., 0. ], [ 5., -115., 0. ] ] }
+   - { id:529, corners:[ [ 115., -215., 0. ], [ 215., -215., 0. ], [
+       215., -115., 0. ], [ 115., -115., 0. ] ] }
+   - { id:226, corners:[ [ -215., -105., 0. ], [ -115., -105., 0. ], [
+       -115., -5., 0. ], [ -215., -5., 0. ] ] }
+   - { id:886, corners:[ [ -105., -105., 0. ], [ -5., -105., 0. ], [ -5.,
+       -5., 0. ], [ -105., -5., 0. ] ] }
+   - { id:401, corners:[ [ 5., -105., 0. ], [ 105., -105., 0. ], [ 105.,
+       -5., 0. ], [ 5., -5., 0. ] ] }
+   - { id:976, corners:[ [ 115., -105., 0. ], [ 215., -105., 0. ], [
+       215., -5., 0. ], [ 115., -5., 0. ] ] }
+   - { id:55, corners:[ [ -215., 5., 0. ], [ -115., 5., 0. ], [ -115.,
+       105., 0. ], [ -215., 105., 0. ] ] }
+   - { id:685, corners:[ [ -105., 5., 0. ], [ -5., 5., 0. ], [ -5., 105.,
+       0. ], [ -105., 105., 0. ] ] }
+   - { id:53, corners:[ [ 5., 5., 0. ], [ 105., 5., 0. ], [ 105., 105.,
+       0. ], [ 5., 105., 0. ] ] }
+   - { id:91, corners:[ [ 115., 5., 0. ], [ 215., 5., 0. ], [ 215., 105.,
+       0. ], [ 115., 105., 0. ] ] }
+   - { id:97, corners:[ [ -215., 115., 0. ], [ -115., 115., 0. ], [
+       -115., 215., 0. ], [ -215., 215., 0. ] ] }
+   - { id:773, corners:[ [ -105., 115., 0. ], [ -5., 115., 0. ], [ -5.,
+       215., 0. ], [ -105., 215., 0. ] ] }
+   - { id:453, corners:[ [ 5., 115., 0. ], [ 105., 115., 0. ], [ 105.,
+       215., 0. ], [ 5., 215., 0. ] ] }
+   - { id:471, corners:[ [ 115., 115., 0. ], [ 215., 115., 0. ], [ 215.,
+       215., 0. ], [ 115., 215., 0. ] ] }
+   - { id:752, corners:[ [ -215., 225., 0. ], [ -115., 225., 0. ], [
+       -115., 325., 0. ], [ -215., 325., 0. ] ] }
+   - { id:273, corners:[ [ -105., 225., 0. ], [ -5., 225., 0. ], [ -5.,
+       325., 0. ], [ -105., 325., 0. ] ] }
+   - { id:983, corners:[ [ 5., 225., 0. ], [ 105., 225., 0. ], [ 105.,
+       325., 0. ], [ 5., 325., 0. ] ] }
+   - { id:298, corners:[ [ 115., 225., 0. ], [ 215., 225., 0. ], [ 215.,
+       325., 0. ], [ 115., 325., 0. ] ] }

data/single/pose_calib_01.yml

@@ -0,0 +1,32 @@
+%YAML:1.0
+aruco_bc_nmarkers: 12
+aruco_bc_mInfoType: 0
+aruco_bc_markers:
+   - { id:708, corners:[ [ -350., -250., 0. ], [ -250., -250., 0. ], [
+       -250., -150., 0. ], [ -350., -150., 0. ] ] }
+   - { id:386, corners:[ [ -150., -250., 0. ], [ -50., -250., 0. ], [ -50.,
+       -150., 0. ], [ -150., -150., 0. ] ] }
+   - { id:839, corners:[ [ 50., -250., 0. ], [ 150., -250., 0. ], [ 150.,
+       -150., 0. ], [ 50., -150., 0. ] ] }
+   - { id:674, corners:[ [ 250., -250., 0. ], [ 350., -250, 0. ], [
+       350., -150., 0. ], [ 250., -150., 0. ] ] }
+
+
+   - { id:479, corners:[ [ -350., -50., 0. ], [ -250., -50., 0. ], [
+       -250., 50., 0. ], [ -350., 50., 0. ] ] }
+   - { id:764, corners:[ [ -150., -50., 0. ], [ -50., -50., 0. ], [ -50.,
+       50., 0. ], [ -150., 50., 0. ] ] }
+   - { id:126, corners:[ [ 50., -50., 0. ], [ 150., -50., 0. ], [ 150.,
+       50., 0. ], [ 50., 50., 0. ]  ] }
+   - { id:903, corners:[ [ 250., -50., 0. ], [ 350., -50., 0. ], [
+       350., 50., 0. ], [ 250., 50., 0. ] ] }
+
+   - { id:378, corners:[ [ -350., 150., 0. ], [ -250., 150., 0. ], [
+       -250., 250., 0. ], [ -350., 250., 0. ] ] }
+   - { id:100, corners:[ [ -150., 150., 0. ], [ -50., 150., 0. ], [ -50.,
+       250., 0. ], [ -150., 250., 0. ] ] }
+   - { id:622, corners:[ [ 50., 150., 0. ], [ 150., 150., 0. ], [ 150.,
+       250., 0. ], [ 50., 250., 0. ] ] }
+   - { id:402, corners:[ [ 250., 150., 0. ], [ 350., 150., 0. ], [
+       350., 250., 0. ], [ 250., 250., 0. ] ] }
+

include/calibrate_mocap_and_camera/calibrate_mocap_and_camera.h

@@ -0,0 +1,74 @@
+/* 
+ * File:   Feature3DEngine.h
+ * Author: arwillis
+ *
+ * Created on August 18, 2015, 10:35 AM
+ */
+
+#ifndef CALIBRATE_MOCAP_AND_CAMERA_H
+#define CALIBRATE_MOCAP_AND_CAMERA_H
+
+// Standard C++ includes
+#include <string>
+#include <iostream>
+#include <fstream>
+#include <math.h>
+
+// ROS includes
+#include <ros/ros.h>
+#include <message_filters/subscriber.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Header.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+
+class CalibrateMocapAndCamera {
+public:
+    typedef boost::shared_ptr<CalibrateMocapAndCamera> Ptr;
+
+    CalibrateMocapAndCamera(std::string optical_parent, std::string optical_frame,
+    bool _logdata, std::string _logfilename) :
+    map_frame_id_str(optical_parent),
+    rgb_frame_id_str(optical_frame),
+    logdata(_logdata),
+    logfilename(_logfilename) {
+        if (_logdata) {
+            std::cout << "Opening logfile " << _logfilename << "." << std::endl;
+            fos.open(_logfilename.c_str());
+        }
+    }
+
+    virtual ~CalibrateMocapAndCamera() {
+        if (logdata) {
+            fos.close();
+        }
+    }
+
+    void initializeSubscribers(ros::NodeHandlePtr nodeptr,
+            std::string tf_camera_topic, std::string tf_calib_topic, std::string ar_calib_topic, int timeval = 10) {
+        sub_tf_aruco_calib_pose = nodeptr->subscribe(ar_calib_topic, 1, &CalibrateMocapAndCamera::ar_calib_pose_Callback, this);
+    }
+    void ar_calib_pose_Callback(const geometry_msgs::TransformStampedConstPtr& ar_calib_pose);
+
+
+private:
+    // -------------------------
+    // Disabling default copy constructor and default
+    // assignment operator.
+    // -------------------------
+    CalibrateMocapAndCamera(const CalibrateMocapAndCamera& yRef);
+    CalibrateMocapAndCamera& operator=(const CalibrateMocapAndCamera& yRef);
+
+
+    // variable used when using a ground truth reference initialization
+    ros::Subscriber sub_tf_aruco_calib_pose;
+
+    std::string map_frame_id_str;
+    std::string rgb_frame_id_str;
+
+    bool logdata;
+    std::string logfilename;
+    std::ofstream fos;
+};
+#endif /* CALIBRATE_MOCAP_AND_CAMERA_H */
+