From 7120f01493a19a7c9b21b488c38c34953532ba63 Mon Sep 17 00:00:00 2001 From: ZhenghaoFei Date: Wed, 1 May 2019 19:12:39 -0700 Subject: [PATCH] init ros version --- ROS/src/CMakeLists.txt | 63 ++++ ROS/src/visual_odometry/CMakeLists.txt | 207 ++++++++++++ .../visual_odometry/include/VisualOdometer.h | 85 +++++ ROS/src/visual_odometry/package.xml | 88 +++++ ROS/src/visual_odometry/src/CMakeLists.txt | 37 +++ .../visual_odometry/src/VisualOdometer.cpp | 307 ++++++++++++++++++ .../visual_odometry/src/visual_odom_node.cpp | 36 ++ ROS/working.sh | 2 + calibration/zed_1080p.yaml | 25 ++ src/bucket.cpp | 2 +- src/feature.cpp | 1 + src/visualOdometry.cpp | 14 +- src/visualOdometry.h | 4 +- 13 files changed, 864 insertions(+), 7 deletions(-) create mode 100644 ROS/src/CMakeLists.txt create mode 100644 ROS/src/visual_odometry/CMakeLists.txt create mode 100644 ROS/src/visual_odometry/include/VisualOdometer.h create mode 100644 ROS/src/visual_odometry/package.xml create mode 100644 ROS/src/visual_odometry/src/CMakeLists.txt create mode 100644 ROS/src/visual_odometry/src/VisualOdometer.cpp create mode 100644 ROS/src/visual_odometry/src/visual_odom_node.cpp create mode 100644 ROS/working.sh create mode 100644 calibration/zed_1080p.yaml diff --git a/ROS/src/CMakeLists.txt b/ROS/src/CMakeLists.txt new file mode 100644 index 0000000..2978ef0 --- /dev/null +++ b/ROS/src/CMakeLists.txt @@ -0,0 +1,63 @@ +# toplevel CMakeLists.txt for a catkin workspace +# catkin/cmake/toplevel.cmake + +cmake_minimum_required(VERSION 2.8.3) + +set(CATKIN_TOPLEVEL TRUE) + +# search for catkin within the workspace +set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") +execute_process(COMMAND ${_cmd} + RESULT_VARIABLE _res + OUTPUT_VARIABLE _out + ERROR_VARIABLE _err + OUTPUT_STRIP_TRAILING_WHITESPACE + ERROR_STRIP_TRAILING_WHITESPACE +) +if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) + # searching fot catkin resulted in an error + string(REPLACE ";" " " _cmd_str "${_cmd}") + message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") +endif() + +# include catkin from workspace or via find_package() +if(_res EQUAL 0) + set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") + # include all.cmake without add_subdirectory to let it operate in same scope + include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) + add_subdirectory("${_out}") + +else() + # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument + # or CMAKE_PREFIX_PATH from the environment + if(NOT DEFINED CMAKE_PREFIX_PATH) + if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") + string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) + endif() + endif() + + # list of catkin workspaces + set(catkin_search_path "") + foreach(path ${CMAKE_PREFIX_PATH}) + if(EXISTS "${path}/.catkin") + list(FIND catkin_search_path ${path} _index) + if(_index EQUAL -1) + list(APPEND catkin_search_path ${path}) + endif() + endif() + endforeach() + + # search for catkin in all workspaces + set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) + find_package(catkin QUIET + NO_POLICY_SCOPE + PATHS ${catkin_search_path} + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) + unset(CATKIN_TOPLEVEL_FIND_PACKAGE) + + if(NOT catkin_FOUND) + message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") + endif() +endif() + +catkin_workspace() diff --git a/ROS/src/visual_odometry/CMakeLists.txt b/ROS/src/visual_odometry/CMakeLists.txt new file mode 100644 index 0000000..a85afd1 --- /dev/null +++ b/ROS/src/visual_odometry/CMakeLists.txt @@ -0,0 +1,207 @@ +cmake_minimum_required(VERSION 2.8.3) +project(visual_odometry) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## 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 + geometry_msgs + nav_msgs + roscpp + rospy + std_msgs + message_filters + image_transport + cv_bridge + tf +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## 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 exec_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 exec_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 +# geometry_msgs# nav_msgs# std_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 exec_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 your 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 visual_odometry +# CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_subdirectory(src) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/visual_odometry.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(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/visual_odometry_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_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 ${PROJECT_NAME} ${PROJECT_NAME}_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_visual_odometry.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) diff --git a/ROS/src/visual_odometry/include/VisualOdometer.h b/ROS/src/visual_odometry/include/VisualOdometer.h new file mode 100644 index 0000000..3e2aa22 --- /dev/null +++ b/ROS/src/visual_odometry/include/VisualOdometer.h @@ -0,0 +1,85 @@ +#ifndef VISUAL_ODOMETER_H +#define VISUAL_ODOMETER_H + +// ROS include +#include "ros/ros.h" +#include "sensor_msgs/Image.h" +#include "sensor_msgs/CameraInfo.h" +#include "nav_msgs/Odometry.h" +#include "std_msgs/Time.h" + +#include +#include +#include +#include + +#include "opencv2/video/tracking.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/calib3d/calib3d.hpp" + +#include "feature.h" +#include "utils.h" +#include "evaluate_odometry.h" +#include "visualOdometry.h" +#include "Frame.h" +#include + + +class VisualOdometer +{ +public: + + VisualOdometer(ros::NodeHandle& nh); + ~VisualOdometer(); + void imageGrabCallback(const sensor_msgs::ImageConstPtr& left_image_msg_ptr, const sensor_msgs::ImageConstPtr& right_image_msg_ptr, + const sensor_msgs::CameraInfoConstPtr& left_cam_info_msg_ptr, const sensor_msgs::CameraInfoConstPtr& right_cam_info_msg_ptr); + + cv::Mat transInCameraFrame(cv::Mat& rotation, cv::Mat& translation); + + void tracking(cv::Mat& image_left, cv::Mat& image_right, ros::Time& stamp); + + bool checkValidTrans(cv::Mat& rotation, cv::Mat& translation); + + + void integrateOdometry(cv::Mat& frame_pose, cv::Mat& trans); + void constructOdomMsg(ros::Time stamp, cv::Mat& frame_pose, cv::Mat& rotation, cv::Mat& translation, float dt); + void staticTF(); + +private: + ros::Publisher pub_odom; + + cv::Mat projMatrl; + cv::Mat projMatrr; + bool projection_mat_initialized; + + cv::Mat image_left_last, image_right_last; + ros::Time stamp_last; + bool vo_initialized; + + + FeatureSet currentVOFeatures; + std::vector oldFeaturePointsLeft; + std::vector currentFeaturePointsLeft; + + // ----------------------------------------- + // Initialize variables + // ----------------------------------------- + // cv::Mat rotation = cv::Mat::eye(3, 3, CV_64F); + // cv::Mat translation_stereo = cv::Mat::zeros(3, 1, CV_64F); + + cv::Mat pose = cv::Mat::zeros(3, 1, CV_64F); + cv::Mat Rpose = cv::Mat::eye(3, 3, CV_64F); + + cv::Mat frame_pose = cv::Mat::eye(4, 4, CV_64F); + + cv::Mat trajectory = cv::Mat::zeros(600, 1200, CV_8UC3); + cv::Mat points4D, points3D; + int frame_id = 0; + +}; + + +#endif + diff --git a/ROS/src/visual_odometry/package.xml b/ROS/src/visual_odometry/package.xml new file mode 100644 index 0000000..fe4cdef --- /dev/null +++ b/ROS/src/visual_odometry/package.xml @@ -0,0 +1,88 @@ + + + visual_odometry + 0.0.0 + The visual_odometry package + + + + + zfei + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + nav_msgs + roscpp + rospy + std_msgs + image_transport + cv_bridge + message_filters + tf + + geometry_msgs + nav_msgs + roscpp + rospy + std_msgs + image_transport + cv_bridge + message_filters + tf + + + geometry_msgs + nav_msgs + roscpp + rospy + std_msgs + image_transport + cv_bridge + message_filters + tf + + + + + + + diff --git a/ROS/src/visual_odometry/src/CMakeLists.txt b/ROS/src/visual_odometry/src/CMakeLists.txt new file mode 100644 index 0000000..265d02e --- /dev/null +++ b/ROS/src/visual_odometry/src/CMakeLists.txt @@ -0,0 +1,37 @@ +set(visual_odom_src "../../../../src/") +message(STATUS "visual_odom_src: " ${visual_odom_src}) + +find_package( OpenCV REQUIRED ) +find_package (Eigen3 REQUIRED NO_MODULE) + + +include_directories(${visual_odom_src}) +include_directories(${visual_odom_src}/evaluate) +include_directories(${OpenCV_INCLUDE_DIRS} ) +include_directories (${EIGEN3_INCLUDE_DIR}) + + +add_library(evaluate_odometry SHARED ${visual_odom_src}evaluate/evaluate_odometry.cpp) +add_library(matrix SHARED ${visual_odom_src}evaluate/matrix.cpp) +add_library(feature SHARED ${visual_odom_src}feature.cpp) +add_library(bucket SHARED ${visual_odom_src}bucket.cpp) +add_library(utils SHARED ${visual_odom_src}utils.cpp) +add_library(visualOdometry SHARED ${visual_odom_src}visualOdometry.cpp) +add_library(Frame SHARED ${visual_odom_src}Frame.cpp) + + +target_link_libraries( evaluate_odometry matrix) +target_link_libraries( bucket ${OpenCV_LIBS} ) +target_link_libraries( feature ${OpenCV_LIBS} bucket) +target_link_libraries( utils evaluate_odometry feature ${OpenCV_LIBS} ) +target_link_libraries( visualOdometry utils bucket feature ${OpenCV_LIBS}) +target_link_libraries( Frame ${OpenCV_LIBS} ) + + +add_library(VisualOdometer SHARED "VisualOdometer.cpp") +target_link_libraries (VisualOdometer ${catkin_LIBRARIES} ${OpenCV_LIBS} bucket feature utils visualOdometry Frame) + +add_executable(runVisualOdometry "visual_odom_node.cpp") +target_link_libraries(runVisualOdometry VisualOdometer ${catkin_LIBRARIES}) + +# add_dependencies(runPointcloudPreProcessor template_in_row_localization_generate_messages_cpp) diff --git a/ROS/src/visual_odometry/src/VisualOdometer.cpp b/ROS/src/visual_odometry/src/VisualOdometer.cpp new file mode 100644 index 0000000..2a23389 --- /dev/null +++ b/ROS/src/visual_odometry/src/VisualOdometer.cpp @@ -0,0 +1,307 @@ +#include "VisualOdometer.h" + + +VisualOdometer::VisualOdometer(ros::NodeHandle& nh) +{ + // publisher + pub_odom = nh.advertise ("/vo/odom", 1); + projection_mat_initialized = false; + vo_initialized = false; + staticTF(); +} + +void VisualOdometer::staticTF() +{ + static tf2_ros::StaticTransformBroadcaster static_broadcaster; + geometry_msgs::TransformStamped static_transformStamped; + + static_transformStamped.header.stamp = ros::Time::now(); + static_transformStamped.header.frame_id = "odom"; + static_transformStamped.child_frame_id = "camera_odom"; + static_transformStamped.transform.translation.x = 0.; + static_transformStamped.transform.translation.y = 0.; + static_transformStamped.transform.translation.z = 0.; + tf2::Quaternion quat; + quat.setRPY(-M_PI/2, 0, -M_PI/2); + static_transformStamped.transform.rotation.x = quat.x(); + static_transformStamped.transform.rotation.y = quat.y(); + static_transformStamped.transform.rotation.z = quat.z(); + static_transformStamped.transform.rotation.w = quat.w(); + static_broadcaster.sendTransform(static_transformStamped); +} + +cv::Mat VisualOdometer::transInCameraFrame(cv::Mat& rotation, cv::Mat& translation) +{ + // The visual odometey solvePnp give rotation and translation in camera frame (z forwarding), + // transform it into x forawrd + + cv::Mat_ T_in_cam_opt = (cv::Mat_(4, 4) << rotation.at(0), rotation.at(1), rotation.at(2), translation.at(0), + rotation.at(3), rotation.at(4), rotation.at(5), translation.at(1), + rotation.at(6), rotation.at(7), rotation.at(8), translation.at(2), + 0, 0, 0, 1); + + cv::Mat_ cam_T_cam_opt = (cv::Mat_(4, 4) << 0, 0, 1, 0, + -1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, 0, 1); + + cv::Mat_ T_in_cam = cam_T_cam_opt * T_in_cam_opt * cam_T_cam_opt.inv(); + + return T_in_cam; + +} + + +void VisualOdometer::integrateOdometry(cv::Mat& frame_pose, cv::Mat& trans) +{ + frame_pose = frame_pose * trans; +} + +void VisualOdometer::constructOdomMsg(ros::Time stamp, cv::Mat& frame_pose, cv::Mat& rotation, cv::Mat& translation, float dt) +{ + + cv::Mat pose_rotation_matrix = frame_pose(cv::Range(0, 3), cv::Range(0, 3)); + // std::cout << "pose_rotation_matrix " << pose_rotation_matrix << std::endl; + + cv::Vec3f pose_rotation_euler = rotationMatrixToEulerAngles(pose_rotation_matrix); + std::cout << "pose_rotation_euler " << pose_rotation_euler << std::endl; + + geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(double(pose_rotation_euler[0]), double(pose_rotation_euler[1]), double(pose_rotation_euler[2])); + geometry_msgs::TransformStamped odom_trans; + + nav_msgs::Odometry odom; + odom.header.stamp = stamp; + odom.header.frame_id = "odom"; + + //set the position + odom.pose.pose.position.x = frame_pose.at(3); + odom.pose.pose.position.y = frame_pose.at(7); + odom.pose.pose.position.z = frame_pose.at(11); + odom.pose.pose.orientation = odom_quat; + + //set the velocity + odom.child_frame_id = "camera"; + // odom.twist.twist.linear.x = vx; + // odom.twist.twist.linear.y = vy; + // odom.twist.twist.angular.z = vth; + pub_odom.publish(odom); + +} + + +bool VisualOdometer::checkValidTrans(cv::Mat& rotation, cv::Mat& translation) +{ + + bool valid = true; + + // Check rotation + cv::Vec3f rotation_euler = rotationMatrixToEulerAngles(rotation); + + if(abs(rotation_euler[1])>0.1 || abs(rotation_euler[0])>0.1 || abs(rotation_euler[2])>0.1) + { + std::cout << "[WARNING] Too large rotation" << std::endl; + valid = false; + } + + // Check translation + double scale = sqrt((translation.at(3))*(translation.at(3)) + + (translation.at(7))*(translation.at(7)) + + (translation.at(11))*(translation.at(11))) ; + + std::cout << "scale: " << scale << std::endl; + + if (scale <= 0.0 || scale > 10) + { + std::cout << "[WARNING] scale below 0.1, or incorrect translation" << std::endl; + valid = false; + } + + return valid; +} + + +void VisualOdometer::tracking(cv::Mat& image_left, cv::Mat& image_right, ros::Time& stamp) +{ frame_id += 1; + if(!vo_initialized) + { + image_left_last = image_left; + image_right_last = image_right; + stamp_last = stamp; + vo_initialized = true; + std::cout << "vo initialized " << std::endl; + } + else + { + std::vector pointsLeft_t0, pointsRight_t0, pointsLeft_t1, pointsRight_t1; + std::vector oldPointsLeft_t0 = currentVOFeatures.points; + + // std::cout << " image_left_last height width: " << image_left_last.rows << " " < 100) + { + + // --------------------- + // Triangulate 3D Points + // --------------------- + cv::Mat points3D_t0, points4D_t0; + cv::triangulatePoints( projMatrl, projMatrr, pointsLeft_t0, pointsRight_t0, points4D_t0); + cv::convertPointsFromHomogeneous(points4D_t0.t(), points3D_t0); + + cv::Mat points3D_t1, points4D_t1; + cv::triangulatePoints( projMatrl, projMatrr, pointsLeft_t1, pointsRight_t1, points4D_t1); + cv::convertPointsFromHomogeneous(points4D_t1.t(), points3D_t1); + + // --------------------- + // Tracking transfomation + // --------------------- + cv::Mat rotation = cv::Mat::eye(3, 3, CV_64F); + cv::Mat translation = cv::Mat::zeros(3, 1, CV_64F); + + + // rotation = (cv::Mat_(3, 3) << 0, 0, 1, + // -0, 1, 0, + // -1, 0, 0); + + // translation = (cv::Mat_(3, 1) << 1, 2, 3); + // std::cout << "rotation: " << rotation << std::endl; + // std::cout << "translation: " << translation << std::endl; + + + // std::cout << "trans: " << trans << std::endl; + + + trackingFrame2Frame(projMatrl, projMatrr, pointsLeft_t0, pointsLeft_t1, points3D_t0, rotation, translation); + cv::Mat trans = transInCameraFrame(rotation, translation); + + // translation = -translation; + + // displayTracking(image_left, pointsLeft_t0, pointsLeft_t1); + // cv::waitKey(1); + + + // ------------------------------------------------ + // Intergrating and display + // ------------------------------------------------ + bool trans_valid = checkValidTrans(rotation, translation); + + + // std::cout << "rotation: " << rotation_euler << std::endl; + // std::cout << "translation: " << translation.t() << std::endl; + + if(trans_valid) + { + integrateOdometry(frame_pose, trans); + constructOdomMsg(stamp, frame_pose, rotation, translation, delta_t); + + } + + std::cout << "frame_pose" << frame_pose << std::endl; + + + // // std::cout << "rigid_body_transformation" << rigid_body_transformation << std::endl; + + // // std::cout << "frame_pose" << frame_pose << std::endl; + + + // Rpose = frame_pose(cv::Range(0, 3), cv::Range(0, 3)); + // cv::Vec3f Rpose_euler = rotationMatrixToEulerAngles(Rpose); + // // std::cout << "Rpose_euler" << Rpose_euler << std::endl; + + // cv::Mat pose = frame_pose.col(3).clone(); + + + // std::cout << "Pose: " << pose.t() << std::endl; + + } + else + { + std::cerr << "[]WARNING] Matched feature cirtically low" << pose.t() << std::endl; + + } + + } + + +} + +void VisualOdometer::imageGrabCallback(const sensor_msgs::ImageConstPtr& left_image_msg_ptr, const sensor_msgs::ImageConstPtr& right_image_msg_ptr, + const sensor_msgs::CameraInfoConstPtr& left_cam_info_msg_ptr, const sensor_msgs::CameraInfoConstPtr& right_cam_info_msg_ptr) + +{ + + std::cout << std::endl; + ros::Time stamp = left_image_msg_ptr->header.stamp; + std::cout << "Recived Image Pair at: " << stamp << std::endl; + // std::cout << "left: " << left_image_msg_ptr->header.stamp << std::endl; + // std::cout << "right: " << right_image_msg_ptr->header.stamp << std::endl; + + if(!projection_mat_initialized) + { + projMatrl = cv::Mat(3, 4, CV_64FC1, (void *) left_cam_info_msg_ptr->P.data()); + projMatrr = cv::Mat(3, 4, CV_64FC1, (void *) right_cam_info_msg_ptr->P.data()); + projMatrl.convertTo(projMatrl, CV_32F); + projMatrr.convertTo(projMatrr, CV_32F); + + std::cout << "projMatrl " << projMatrl << std::endl; + std::cout << "projMatrr " << projMatrr << std::endl; + projection_mat_initialized = true; + } + else + { + // Copy the ros image message to cv::Mat. + cv_bridge::CvImageConstPtr cv_ptrLeft; + try + { + cv_ptrLeft = cv_bridge::toCvShare(left_image_msg_ptr); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + cv_bridge::CvImageConstPtr cv_ptrRight; + try + { + cv_ptrRight = cv_bridge::toCvShare(right_image_msg_ptr); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + + cv::Mat image_left, image_right; + cvtColor(cv_ptrLeft->image, image_left, cv::COLOR_BGR2GRAY); + cvtColor(cv_ptrRight->image, image_right, cv::COLOR_BGR2GRAY); + + // mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); + tracking(image_left, image_right, stamp); + } +} + +VisualOdometer::~VisualOdometer() +{ + +} + diff --git a/ROS/src/visual_odometry/src/visual_odom_node.cpp b/ROS/src/visual_odometry/src/visual_odom_node.cpp new file mode 100644 index 0000000..8744a37 --- /dev/null +++ b/ROS/src/visual_odometry/src/visual_odom_node.cpp @@ -0,0 +1,36 @@ + +#include +#include "VisualOdometer.h" +#include +#include +#include +#include +#include + +int main(int argc, char **argv) +{ + + ros::init(argc, argv, "visual_odom_node"); + ros::NodeHandle nh; + + VisualOdometer visual_odometer(nh); + + image_transport::ImageTransport it(nh); + + + image_transport::SubscriberFilter left_image_sub(it, "/zed/left/image_rect_color", 1); + image_transport::SubscriberFilter right_image_sub(it, "/zed/right/image_rect_color", 1); + // image_transport::SubscriberFilter right_cam_info_sub(it, "/zed/right/camera_info", 1); + message_filters::Subscriber left_cam_info_sub(nh, "/zed/left/camera_info", 1); + message_filters::Subscriber right_cam_info_sub(nh, "/zed/right/camera_info", 1); + + + + message_filters::TimeSynchronizer sync(left_image_sub, right_image_sub, left_cam_info_sub, right_cam_info_sub, 10); + + sync.registerCallback(boost::bind(&VisualOdometer::imageGrabCallback, &visual_odometer, _1, _2, _3, _4)); + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/ROS/working.sh b/ROS/working.sh new file mode 100644 index 0000000..cdadec2 --- /dev/null +++ b/ROS/working.sh @@ -0,0 +1,2 @@ +cd /home/zfei/Documents/temp/visual_odom/ROS +source devel/setup.bash \ No newline at end of file diff --git a/calibration/zed_1080p.yaml b/calibration/zed_1080p.yaml new file mode 100644 index 0000000..2ce5291 --- /dev/null +++ b/calibration/zed_1080p.yaml @@ -0,0 +1,25 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 684.367919921875 +Camera.fy: 684.367919921875 +Camera.cx: 689.889404296875 +Camera.cy: 406.87420654296875 + +Camera.width: 720 +Camera.height: 1280 + +# Camera frames per second +Camera.fps: 10.0 + +# stereo baseline times fx +Camera.bf: -82.12415128946304 + + +# Close/Far threshold. Baseline times. +ThDepth: 35 + diff --git a/src/bucket.cpp b/src/bucket.cpp index 4661c1d..cd69531 100644 --- a/src/bucket.cpp +++ b/src/bucket.cpp @@ -13,7 +13,7 @@ int Bucket::size(){ void Bucket::add_feature(cv::Point2f point, int age){ // won't add feature with age > 10; - int age_threshold = 10; + int age_threshold = 20; if (age < age_threshold) { // insert any feature before bucket is full diff --git a/src/feature.cpp b/src/feature.cpp index 911cafc..3a906b9 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -143,6 +143,7 @@ void bucketingFeatures(cv::Mat& image, FeatureSet& current_features, int bucket_ int buckets_nums_height = image_height/bucket_size; int buckets_nums_width = image_width/bucket_size; int buckets_number = buckets_nums_height * buckets_nums_width; + std::cout << "buckets_number: " < Buckets; diff --git a/src/visualOdometry.cpp b/src/visualOdometry.cpp index d670134..bd07226 100644 --- a/src/visualOdometry.cpp +++ b/src/visualOdometry.cpp @@ -84,7 +84,9 @@ void matchingFeatures(cv::Mat& imageLeft_t0, cv::Mat& imageRight_t0, std::vector& pointsLeft_t0, std::vector& pointsRight_t0, std::vector& pointsLeft_t1, - std::vector& pointsRight_t1) + std::vector& pointsRight_t1, + int bucket_size, + int features_per_bucket) { // ---------------------------- // Feature detection using FAST @@ -97,14 +99,15 @@ void matchingFeatures(cv::Mat& imageLeft_t0, cv::Mat& imageRight_t0, // append new features with old features appendNewFeatures(imageLeft_t0, currentVOFeatures); - // std::cout << "Current feature set size: " << currentVOFeatures.points.size() << std::endl; + std::cout << "New feature set size: " << currentVOFeatures.points.size() << std::endl; } // -------------------------------------------------------- // Feature tracking using KLT tracker, bucketing and circular matching // -------------------------------------------------------- - int bucket_size = 50; - int features_per_bucket = 4; + std::cout << "Current bucketingFeatures set size: " << currentVOFeatures.points.size() << std::endl; + + bucketingFeatures(imageLeft_t0, currentVOFeatures, bucket_size, features_per_bucket); pointsLeft_t0 = currentVOFeatures.points; @@ -146,7 +149,8 @@ void trackingFrame2Frame(cv::Mat& projMatrl, cv::Mat& projMatrr, cv::Mat translation_mono = cv::Mat::zeros(3, 1, CV_64F); E = cv::findEssentialMat(pointsLeft_t1, pointsLeft_t0, focal, principle_point, cv::RANSAC, 0.999, 1.0, mask); cv::recoverPose(E, pointsLeft_t1, pointsLeft_t0, rotation, translation_mono, focal, principle_point, mask); - // std::cout << "recoverPose rotation: " << rotation << std::endl; + std::cout << "recoverPose rotation: " << rotation << std::endl; + std::cout << "translation_mono: " << translation_mono << std::endl; // ------------------------------------------------ // Translation (t) estimation by use solvePnPRansac diff --git a/src/visualOdometry.h b/src/visualOdometry.h index 291d86b..6b01b8a 100644 --- a/src/visualOdometry.h +++ b/src/visualOdometry.h @@ -49,7 +49,9 @@ void matchingFeatures(cv::Mat& imageLeft_t0, cv::Mat& imageRight_t0, std::vector& pointsLeft_t0, std::vector& pointsRight_t0, std::vector& pointsLeft_t1, - std::vector& pointsRight_t1); + std::vector& pointsRight_t1, + int bucket_size=50, + int features_per_bucket=4); void trackingFrame2Frame(cv::Mat& projMatrl, cv::Mat& projMatrr,