From ed4dd67e2ced0ac70040354113d22ce92f976381 Mon Sep 17 00:00:00 2001 From: ebimor Date: Tue, 24 Apr 2018 13:00:39 -0400 Subject: [PATCH 1/6] rectification using openCV --- config/ORB_SLAM_ZED.yaml | 116 +++++ launch/zed_cpu_ros.launch | 24 +- src/zed_cpu_ros.cpp | 898 ++++++++++++++++++-------------------- 3 files changed, 553 insertions(+), 485 deletions(-) create mode 100644 config/ORB_SLAM_ZED.yaml diff --git a/config/ORB_SLAM_ZED.yaml b/config/ORB_SLAM_ZED.yaml new file mode 100644 index 0000000..c22dd65 --- /dev/null +++ b/config/ORB_SLAM_ZED.yaml @@ -0,0 +1,116 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 349.797 +Camera.fy: 349.797 +Camera.cx: 338.78 +Camera.cy: 182.984 + +Camera.k1: -0.175198 +Camera.k2: 0.0283066 +Camera.p1: 0.0 +Camera.p2: 0.0 + +Camera.width: 672 +Camera.height: 376 + +# Camera frames per second +Camera.fps: 30.0 + +# stereo baseline times fx +Camera.bf: 83.95128 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Close/Far threshold. Baseline times. +ThDepth: 35 + +#-------------------------------------------------------------------------------------------- +# Stereo Rectification. Only if you need to pre-rectify the images. +# Camera.fx, .fy, etc must be the same as in LEFT.P +#-------------------------------------------------------------------------------------------- +LEFT.height: 376 +LEFT.width: 672 +LEFT.D: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [-0.175198, 0.0283066, 0.0, 0.0, 0.0] #[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] +LEFT.K: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [349.797, 0.0, 338.78, 0.0, 349.797, 182.984, 0.0, 0.0, 1.0] +LEFT.R: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [0.9998843677510435, 0.0013281419694759294, 0.015148833816660816, -0.001403101601371256, 0.9999868204001661, 0.0049386568884934, -0.01514207492360671, -0.004959341173477632, 0.9998730531933205] +LEFT.P: !!opencv-matrix + rows: 3 + cols: 4 + dt: d + data: [349.797, 0.0, 338.78, 0.0, 0.0, 349.797, 182.984, 0.0, 0.0, 0.0, 1.0, 0.0] + +RIGHT.height: 376 +RIGHT.width: 672 +RIGHT.D: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [-0.169327, 0.0242271, 0.0, 0.0, 0.0] +RIGHT.K: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [349.465, 0.0, 330.883, 0.0, 349.465, 186.47, 0.0, 0.0, 1.0] +RIGHT.R: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [0.9998843677510435, 0.0013281419694759294, 0.015148833816660816, -0.001403101601371256, 0.9999868204001661, 0.0049386568884934, -0.01514207492360671, -0.004959341173477632, 0.9998730531933205] +RIGHT.P: !!opencv-matrix + rows: 3 + cols: 4 + dt: d + data: [349.465, 0.0, 330.883, -41.97564, 0.0, 349.465, 186.47, 0.0, 0.0, 0.0, 1.0, 0.0] + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1200 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize:2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500 + diff --git a/launch/zed_cpu_ros.launch b/launch/zed_cpu_ros.launch index 8250dfa..4538a8e 100644 --- a/launch/zed_cpu_ros.launch +++ b/launch/zed_cpu_ros.launch @@ -1,27 +1,21 @@ + - + + - - - - - - - - - + + - + + - - - + - + diff --git a/src/zed_cpu_ros.cpp b/src/zed_cpu_ros.cpp index ee52d85..630e7bc 100644 --- a/src/zed_cpu_ros.cpp +++ b/src/zed_cpu_ros.cpp @@ -17,496 +17,454 @@ #define HEIGHT_ID 4 #define FPS_ID 5 -namespace arti -{ +namespace arti { + + class StereoCamera { + public: - /** - * @brief { stereo camera driver } - * - * @param[in] resolution The resolution - * @param[in] frame_rate The frame rate - */ - StereoCamera(int device_id, int resolution, double frame_rate) : frame_rate_(30.0) - { - camera_ = new cv::VideoCapture(device_id); - cv::Mat raw; - cv::Mat left_image; - cv::Mat right_image; - setResolution(resolution); - // // this function doesn't work very well in current Opencv 2.4, so, just use ROS to control frame rate. - // setFrameRate(frame_rate); - - ROS_INFO("Stereo Camera Set Resolution %d, width %f, height %f", resolution, camera_->get(WIDTH_ID), - camera_->get(HEIGHT_ID)); - } - - ~StereoCamera() - { - // std::cout << "Destroy the pointer" << std::endl; - delete camera_; - } - - /** - * @brief Sets the resolution. - * - * @param[in] type The type - */ - void setResolution(int type) - { - switch (type) - { - case 0: - width_ = 4416; - height_ = 1242; - break; - case 1: - width_ = 3840; - height_ = 1080; - break; - case 2: - width_ = 2560; - height_ = 720; - break; - case 3: - width_ = 1344; - height_ = 376; - break; - default: - ROS_FATAL("Unknow resolution passed to camera: %d", type); - } - camera_->set(WIDTH_ID, width_); - camera_->set(HEIGHT_ID, height_); - // make sure that the number set are right from the hardware - width_ = camera_->get(WIDTH_ID); - height_ = camera_->get(HEIGHT_ID); - } - - /** - * @brief Sets the frame rate. - * - * @param[in] frame_rate The frame rate - */ - void setFrameRate(double frame_rate) - { - camera_->set(FPS_ID, frame_rate); - frame_rate_ = camera_->get(FPS_ID); - } - - /** - * @brief Gets the images. - * - * @param left_image The left image - * @param right_image The right image - * - * @return The images. - */ - bool getImages(cv::Mat& left_image, cv::Mat& right_image) - { - cv::Mat raw; - if (camera_->grab()) - { - camera_->retrieve(raw); - cv::Rect left_rect(0, 0, width_ / 2, height_); - cv::Rect right_rect(width_ / 2, 0, width_ / 2, height_); - left_image = raw(left_rect); - right_image = raw(right_rect); - cv::waitKey(10); - return true; - } - else - { - return false; - } - } + /** + * @brief { stereo camera driver } + * + * @param[in] resolution The resolution + * @param[in] frame_rate The frame rate + */ + StereoCamera(int resolution, double frame_rate): frame_rate_(30.0) { + + camera_ = new cv::VideoCapture(1); + cv::Mat raw; + cv::Mat left_image; + cv::Mat right_image; + setResolution(resolution); + // // this function doesn't work very well in current Opencv 2.4, so, just use ROS to control frame rate. + // setFrameRate(frame_rate); + + std::cout << "Stereo Camera Set Resolution: " << camera_->get(WIDTH_ID) << "x" << camera_->get(HEIGHT_ID) << std::endl; + // std::cout << "Stereo Camera Set Frame Rate: " << camera_->get(FPS_ID) << std::endl; + } + + ~StereoCamera() { + // std::cout << "Destroy the pointer" << std::endl; + delete camera_; + } + + /** + * @brief Sets the resolution. + * + * @param[in] type The type + */ + void setResolution(int type) { + + if (type == 0) { width_ = 4416; height_ = 1242;} // 2K + if (type == 1) { width_ = 3840; height_ = 1080;} // FHD + if (type == 2) { width_ = 2560; height_ = 720;} // HD + if (type == 3) { width_ = 1344; height_ = 376;} // VGA + + camera_->set(WIDTH_ID, width_); + camera_->set(HEIGHT_ID, height_); + + // make sure that the number set are right from the hardware + width_ = camera_->get(WIDTH_ID); + height_ = camera_->get(HEIGHT_ID); + + } + + /** + * @brief Sets the frame rate. + * + * @param[in] frame_rate The frame rate + */ + void setFrameRate(double frame_rate) { + camera_->set(FPS_ID, frame_rate); + frame_rate_ = camera_->get(FPS_ID); + } + + /** + * @brief Gets the images. + * + * @param left_image The left image + * @param right_image The right image + * + * @return The images. + */ + bool getImages(cv::Mat& left_image, cv::Mat& right_image) { + cv::Mat raw; + if (camera_->grab()) { + camera_->retrieve(raw); + cv::Rect left_rect(0, 0, width_ / 2, height_); + cv::Rect right_rect(width_ / 2, 0, width_ / 2, height_); + left_image = raw(left_rect); + right_image = raw(right_rect); + cv::waitKey(10); + return true; + } else { + return false; + } + } private: - cv::VideoCapture* camera_; - int width_; - int height_; - double frame_rate_; - bool cv_three_; + cv::VideoCapture* camera_; + int width_; + int height_; + double frame_rate_; + bool cv_three_; }; /** * @brief the camera ros warpper class */ -class ZedCameraROS -{ +class ZedCameraROS { public: - /** - * @brief { function_description } - * - * @param[in] resolution The resolution - * @param[in] frame_rate The frame rate - */ - ZedCameraROS() - { - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - // get ros param - private_nh.param("resolution", resolution_, 1); - private_nh.param("frame_rate", frame_rate_, 30.0); - private_nh.param("config_file_location", config_file_location_, std::string("")); - private_nh.param("left_frame_id", left_frame_id_, std::string("left_camera")); - private_nh.param("right_frame_id", right_frame_id_, std::string("right_camera")); - private_nh.param("show_image", show_image_, false); - private_nh.param("use_zed_config", use_zed_config_, true); - private_nh.param("device_id", device_id_, 0); - private_nh.param("encoding", encoding_, std::string("brg8")); - - correctFramerate(resolution_, frame_rate_); - - ROS_INFO("Try to initialize the camera"); - StereoCamera zed(device_id_, resolution_, frame_rate_); - ROS_INFO("Initialized the camera"); - - // setup publisher stuff - image_transport::ImageTransport it(nh); - image_transport::Publisher left_image_pub = it.advertise("left/image_raw", 1); - image_transport::Publisher right_image_pub = it.advertise("right/image_raw", 1); - - ros::Publisher left_cam_info_pub = nh.advertise("left/camera_info", 1); - ros::Publisher right_cam_info_pub = nh.advertise("right/camera_info", 1); - - sensor_msgs::CameraInfo left_info, right_info; - - ROS_INFO("Try load camera calibration files"); - if (use_zed_config_) - { - ROS_INFO("Loading from zed calibration files"); - // get camera info from zed - if (!config_file_location_.empty()) - { - try - { - getZedCameraInfo(config_file_location_, resolution_, left_info, right_info); - } - catch (std::runtime_error& e) + + /** + * @brief { function_description } + * + * @param[in] resolution The resolution + * @param[in] frame_rate The frame rate + */ + ZedCameraROS() { + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + // get ros param + private_nh.param("resolution", resolution_, 1); + private_nh.param("frame_rate", frame_rate_, 30.0); + private_nh.param("config_file_location", config_file_location_, std::string("~/SN1267.conf")); + private_nh.param("undistort_config_file_location", undistort_config_file_location_, std::string("~/SN1267.conf")); + private_nh.param("left_frame_id", left_frame_id_, std::string("left_camera")); + private_nh.param("right_frame_id", right_frame_id_, std::string("right_camera")); + private_nh.param("show_image", show_image_, false); + private_nh.param("rectify_image", rectify_image_, true); + private_nh.param("load_zed_config", load_zed_config_, true); + + ROS_INFO("Try to initialize the camera"); + StereoCamera zed(resolution_, frame_rate_); + ROS_INFO("Initialized the camera"); + + // setup publisher stuff + image_transport::ImageTransport it(nh); + image_transport::Publisher left_image_pub = it.advertise("left/image_raw", 1); + image_transport::Publisher right_image_pub = it.advertise("right/image_raw", 1); + + image_transport::Publisher rec_left_image_pub = it.advertise("left/image_rectified", 1); + image_transport::Publisher rec_right_image_pub = it.advertise("right/image_rectified", 1); + + ros::Publisher left_cam_info_pub = nh.advertise("left/camera_info", 1); + ros::Publisher right_cam_info_pub = nh.advertise("right/camera_info", 1); + + sensor_msgs::CameraInfo left_info, right_info; + + + ROS_INFO("Try load camera calibration files"); + if (load_zed_config_) { + ROS_INFO("Loading from zed calibration files"); + // get camera info from zed + try { + getZedCameraInfo(config_file_location_, resolution_, left_info, right_info); + getUndistortedMaps(undistort_config_file_location_); + } + catch (std::runtime_error& e) { + ROS_INFO("Can't load camera info"); + ROS_ERROR("%s", e.what()); + throw e; + } + } else { + ROS_INFO("Loading from ROS calibration files"); + // get config from the left, right.yaml in config + camera_info_manager::CameraInfoManager info_manager(nh); + info_manager.setCameraName("zed/left"); + info_manager.loadCameraInfo( "package://zed_cpu_ros/config/left.yaml"); + left_info = info_manager.getCameraInfo(); + + info_manager.setCameraName("zed/right"); + info_manager.loadCameraInfo( "package://zed_cpu_ros/config/right.yaml"); + right_info = info_manager.getCameraInfo(); + + left_info.header.frame_id = left_frame_id_; + right_info.header.frame_id = right_frame_id_; + } + + // std::cout << left_info << std::endl; + // std::cout << right_info << std::endl; + + ROS_INFO("Got camera calibration files"); + // loop to publish images; + cv::Mat left_image, right_image; + ros::Rate r(frame_rate_); + + while (nh.ok()) { + ros::Time now = ros::Time::now(); + if (!zed.getImages(left_image, right_image)) { + ROS_INFO_ONCE("Can't find camera"); + } else { + ROS_INFO_ONCE("Success, found camera"); + } + if (show_image_) { + cv::imshow("left", left_image); + cv::imshow("right", right_image); + } + + if(rectify_image_){ + cv::Mat imLeftRec, imRightRec; + cv::remap(left_image,imLeftRec,M1l,M2l,cv::INTER_LINEAR); + cv::remap(right_image,imRightRec,M1r,M2r,cv::INTER_LINEAR); + cv::imshow("left_rectified", imLeftRec); + if (rec_left_image_pub.getNumSubscribers() > 0) { + publishImage(imLeftRec, rec_left_image_pub, "rec_left_frame", now); + } + if (rec_right_image_pub.getNumSubscribers() > 0) { + publishImage(imRightRec, rec_right_image_pub, "rec_right_frame", now); + } + } + + if (left_image_pub.getNumSubscribers() > 0) { + publishImage(left_image, left_image_pub, "left_frame", now); + } + if (right_image_pub.getNumSubscribers() > 0) { + publishImage(right_image, right_image_pub, "right_frame", now); + } + if (left_cam_info_pub.getNumSubscribers() > 0) { + publishCamInfo(left_cam_info_pub, left_info, now); + } + if (right_cam_info_pub.getNumSubscribers() > 0) { + publishCamInfo(right_cam_info_pub, right_info, now); + } + r.sleep(); + // since the frame rate was set inside the camera, no need to do a ros sleep + } + } + + /** + * @brief Gets the camera information From Zed config. + * + * @param[in] config_file The configuration file + * @param[in] resolution The resolution + * @param[in] left_cam_info_msg The left camera information message + * @param[in] right_cam_info_msg The right camera information message + */ + void getZedCameraInfo(std::string config_file, int resolution, sensor_msgs::CameraInfo& left_info, sensor_msgs::CameraInfo& right_info) { + boost::property_tree::ptree pt; + boost::property_tree::ini_parser::read_ini(config_file, pt); + std::string left_str = "LEFT_CAM_"; + std::string right_str = "RIGHT_CAM_"; + std::string reso_str = ""; + + switch (resolution) { + case 0: reso_str = "2K"; break; + case 1: reso_str = "FHD"; break; + case 2: reso_str = "HD"; break; + case 3: reso_str = "VGA"; break; + } + // left value + double l_cx = pt.get(left_str + reso_str + ".cx"); + double l_cy = pt.get(left_str + reso_str + ".cy"); + double l_fx = pt.get(left_str + reso_str + ".fx"); + double l_fy = pt.get(left_str + reso_str + ".fy"); + double l_k1 = pt.get(left_str + reso_str + ".k1"); + double l_k2 = pt.get(left_str + reso_str + ".k2"); + // right value + double r_cx = pt.get(right_str + reso_str + ".cx"); + double r_cy = pt.get(right_str + reso_str + ".cy"); + double r_fx = pt.get(right_str + reso_str + ".fx"); + double r_fy = pt.get(right_str + reso_str + ".fy"); + double r_k1 = pt.get(right_str + reso_str + ".k1"); + double r_k2 = pt.get(right_str + reso_str + ".k2"); + + // get baseline and convert mm to m + boost::optional baselineCheck; + double baseline = 0.0; + // some config files have "Baseline" instead of "BaseLine", check accordingly... + if (baselineCheck = pt.get_optional("STEREO.BaseLine")) { + baseline = pt.get("STEREO.BaseLine") * 0.001; + } + else if (baselineCheck = pt.get_optional("STEREO.Baseline")) { + baseline = pt.get("STEREO.Baseline") * 0.001; + } + else + throw std::runtime_error("baseline parameter not found"); + + // get Rx and Rz + double rx = pt.get("STEREO.RX_"+reso_str); + double rz = pt.get("STEREO.RZ_"+reso_str); + double ry = pt.get("STEREO.CV_"+reso_str); + + // assume zeros, maybe not right + double p1 = 0, p2 = 0, k3 = 0; + + left_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + right_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + + // distortion parameters + // For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). + left_info.D.resize(5); + left_info.D[0] = l_k1; + left_info.D[1] = l_k2; + left_info.D[2] = k3; + left_info.D[3] = p1; + left_info.D[4] = p2; + + right_info.D.resize(5); + right_info.D[0] = r_k1; + right_info.D[1] = r_k2; + right_info.D[2] = k3; + right_info.D[3] = p1; + right_info.D[4] = p2; + + // Intrinsic camera matrix + // [fx 0 cx] + // K = [ 0 fy cy] + // [ 0 0 1] + left_info.K.fill(0.0); + left_info.K[0] = l_fx; + left_info.K[2] = l_cx; + left_info.K[4] = l_fy; + left_info.K[5] = l_cy; + left_info.K[8] = 1.0; + + right_info.K.fill(0.0); + right_info.K[0] = r_fx; + right_info.K[2] = r_cx; + right_info.K[4] = r_fy; + right_info.K[5] = r_cy; + right_info.K[8] = 1.0; + + // rectification matrix + // Rl = R_rect, R_r = R * R_rect + // since R is identity, Rl = Rr; + left_info.R.fill(0.0); + right_info.R.fill(0.0); + cv::Mat rvec = (cv::Mat_(3, 1) << rx, ry, rz); + cv::Mat rmat(3, 3, CV_64F); + cv::Rodrigues(rvec, rmat); + int id = 0; + cv::MatIterator_ it, end; + for (it = rmat.begin(); it != rmat.end(); ++it, id++){ + left_info.R[id] = *it; + right_info.R[id] = *it; + } + + // Projection/camera matrix + // [fx' 0 cx' Tx] + // P = [ 0 fy' cy' Ty] + // [ 0 0 1 0] + left_info.P.fill(0.0); + left_info.P[0] = l_fx; + left_info.P[2] = l_cx; + left_info.P[5] = l_fy; + left_info.P[6] = l_cy; + left_info.P[10] = 1.0; + + right_info.P.fill(0.0); + right_info.P[0] = r_fx; + right_info.P[2] = r_cx; + right_info.P[3] = (-1 * l_fx * baseline); + right_info.P[5] = r_fy; + right_info.P[6] = r_cy; + right_info.P[10] = 1.0; + + left_info.width = right_info.width = width_; + left_info.height = right_info.height = height_; + + left_info.header.frame_id = left_frame_id_; + right_info.header.frame_id = right_frame_id_; + + + + } + + + void getUndistortedMaps(std::string config_file){ + + cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); + if(!fsSettings.isOpened()) { - ROS_INFO("Can't load camera info"); - ROS_ERROR("%s", e.what()); - throw e; + std::cerr << "ERROR: Wrong path to settings" << "/n"; + return ; } - } - else - { - ROS_FATAL("Please input zed config file path"); - } - } - else - { - ROS_INFO("Loading from ROS calibration files"); - // here we just use camera infor manager to load info - // get config from the left, right.yaml in config - ros::NodeHandle left_nh("left"); - ros::NodeHandle right_nh("right"); - camera_info_manager::CameraInfoManager left_info_manager(left_nh, "camera/left", - "package://zed_cpu_ros/config/left.yaml"); - left_info = left_info_manager.getCameraInfo(); - - camera_info_manager::CameraInfoManager right_info_manager(right_nh, "camera/right", - "package://zed_cpu_ros/config/right.yaml"); - right_info = right_info_manager.getCameraInfo(); - - left_info.header.frame_id = left_frame_id_; - right_info.header.frame_id = right_frame_id_; - } - ROS_INFO("Got camera calibration files"); - // loop to publish images; - cv::Mat left_image, right_image; - ros::Rate r(frame_rate_); - - while (nh.ok()) - { - ros::Time now = ros::Time::now(); - if (!zed.getImages(left_image, right_image)) - { - ROS_INFO_ONCE("Can't find camera"); - } - else - { - ROS_INFO_ONCE("Success, found camera"); - } - if (show_image_) - { - cv::imshow("left", left_image); - cv::imshow("right", right_image); - } - if (left_image_pub.getNumSubscribers() > 0) - { - publishImage(left_image, left_image_pub, "left_frame", now); - } - if (right_image_pub.getNumSubscribers() > 0) - { - publishImage(right_image, right_image_pub, "right_frame", now); - } - if (left_cam_info_pub.getNumSubscribers() > 0) - { - publishCamInfo(left_cam_info_pub, left_info, now); - } - if (right_cam_info_pub.getNumSubscribers() > 0) - { - publishCamInfo(right_cam_info_pub, right_info, now); - } - r.sleep(); - // since the frame rate was set inside the camera, no need to do a ros sleep - } - } - - /** - * @brief Gets the camera information From Zed config. - * - * @param[in] config_file The configuration file - * @param[in] resolution The resolution - * @param[in] left_cam_info_msg The left camera information message - * @param[in] right_cam_info_msg The right camera information message - */ - void getZedCameraInfo(std::string config_file, int resolution, sensor_msgs::CameraInfo& left_info, - sensor_msgs::CameraInfo& right_info) - { - boost::property_tree::ptree pt; - boost::property_tree::ini_parser::read_ini(config_file, pt); - std::string left_str = "LEFT_CAM_"; - std::string right_str = "RIGHT_CAM_"; - std::string reso_str = ""; - - switch (resolution) - { - case 0: - reso_str = "2K"; - break; - case 1: - reso_str = "FHD"; - break; - case 2: - reso_str = "HD"; - break; - case 3: - reso_str = "VGA"; - break; - } - // left value - double l_cx = pt.get(left_str + reso_str + ".cx"); - double l_cy = pt.get(left_str + reso_str + ".cy"); - double l_fx = pt.get(left_str + reso_str + ".fx"); - double l_fy = pt.get(left_str + reso_str + ".fy"); - double l_k1 = pt.get(left_str + reso_str + ".k1"); - double l_k2 = pt.get(left_str + reso_str + ".k2"); - // right value - double r_cx = pt.get(right_str + reso_str + ".cx"); - double r_cy = pt.get(right_str + reso_str + ".cy"); - double r_fx = pt.get(right_str + reso_str + ".fx"); - double r_fy = pt.get(right_str + reso_str + ".fy"); - double r_k1 = pt.get(right_str + reso_str + ".k1"); - double r_k2 = pt.get(right_str + reso_str + ".k2"); - - // get baseline and convert mm to m - boost::optional baselineCheck; - double baseline = 0.0; - // some config files have "Baseline" instead of "BaseLine", check accordingly... - if (baselineCheck = pt.get_optional("STEREO.BaseLine")) - { - baseline = pt.get("STEREO.BaseLine") * 0.001; - } - else if (baselineCheck = pt.get_optional("STEREO.Baseline")) - { - baseline = pt.get("STEREO.Baseline") * 0.001; - } - else - { - throw std::runtime_error("baseline parameter not found"); - } + cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; + fsSettings["LEFT.K"] >> K_l; + fsSettings["RIGHT.K"] >> K_r; - // get Rx and Rz - double rx = pt.get("STEREO.RX_" + reso_str); - double rz = pt.get("STEREO.RZ_" + reso_str); - double ry = pt.get("STEREO.CV_" + reso_str); - - // assume zeros, maybe not right - double p1 = 0, p2 = 0, k3 = 0; - - left_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - right_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - - // TODO(dizeng) verify loading default zed config is still working - - // distortion parameters - // For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). - left_info.D.resize(5); - left_info.D[0] = l_k1; - left_info.D[1] = l_k2; - left_info.D[2] = k3; - left_info.D[3] = p1; - left_info.D[4] = p2; - - right_info.D.resize(5); - right_info.D[0] = r_k1; - right_info.D[1] = r_k2; - right_info.D[2] = k3; - right_info.D[3] = p1; - right_info.D[4] = p2; - - // Intrinsic camera matrix - // [fx 0 cx] - // K = [ 0 fy cy] - // [ 0 0 1] - left_info.K.fill(0.0); - left_info.K[0] = l_fx; - left_info.K[2] = l_cx; - left_info.K[4] = l_fy; - left_info.K[5] = l_cy; - left_info.K[8] = 1.0; - - right_info.K.fill(0.0); - right_info.K[0] = r_fx; - right_info.K[2] = r_cx; - right_info.K[4] = r_fy; - right_info.K[5] = r_cy; - right_info.K[8] = 1.0; - - // rectification matrix - // Rl = R_rect, R_r = R * R_rect - // since R is identity, Rl = Rr; - left_info.R.fill(0.0); - right_info.R.fill(0.0); - cv::Mat rvec = (cv::Mat_(3, 1) << rx, ry, rz); - cv::Mat rmat(3, 3, CV_64F); - cv::Rodrigues(rvec, rmat); - int id = 0; - cv::MatIterator_ it, end; - for (it = rmat.begin(); it != rmat.end(); ++it, id++) - { - left_info.R[id] = *it; - right_info.R[id] = *it; - } + fsSettings["LEFT.P"] >> P_l; + fsSettings["RIGHT.P"] >> P_r; - // Projection/camera matrix - // [fx' 0 cx' Tx] - // P = [ 0 fy' cy' Ty] - // [ 0 0 1 0] - left_info.P.fill(0.0); - left_info.P[0] = l_fx; - left_info.P[2] = l_cx; - left_info.P[5] = l_fy; - left_info.P[6] = l_cy; - left_info.P[10] = 1.0; - - right_info.P.fill(0.0); - right_info.P[0] = r_fx; - right_info.P[2] = r_cx; - right_info.P[3] = (-1 * l_fx * baseline); - right_info.P[5] = r_fy; - right_info.P[6] = r_cy; - right_info.P[10] = 1.0; - - left_info.width = right_info.width = width_; - left_info.height = right_info.height = height_; - - left_info.header.frame_id = left_frame_id_; - right_info.header.frame_id = right_frame_id_; - } - - /** - * @brief { publish camera info } - * - * @param[in] pub_cam_info The pub camera information - * @param[in] cam_info_msg The camera information message - * @param[in] now The now - */ - void publishCamInfo(const ros::Publisher& pub_cam_info, sensor_msgs::CameraInfo& cam_info_msg, ros::Time now) - { - cam_info_msg.header.stamp = now; - pub_cam_info.publish(cam_info_msg); - } - - /** - * @brief { publish image } - * - * @param[in] img The image - * @param img_pub The image pub - * @param[in] img_frame_id The image frame identifier - * @param[in] t { parameter_description } - * @param[in] encoding image_transport encoding - */ - void publishImage(const cv::Mat& img, image_transport::Publisher& img_pub, const std::string& img_frame_id, - ros::Time t) - { - cv_bridge::CvImage cv_image; - // TODO(dizeng) maybe we can save a copy here? - // or it seems like CV mat is passing by reference? - cv_image.image = img; - // TODO(dizeng) - // by default the cv::mat from zed is bgr8, here just chaing encoding seems - // doesn't work, need to implement conversion function specificly - cv_image.encoding = encoding_; - cv_image.header.frame_id = img_frame_id; - cv_image.header.stamp = t; - img_pub.publish(cv_image.toImageMsg()); - } - /** - * @brief Correct frame rate according to resolution - * - * @param[in] resolution The resolution - * @param frame_rate The camera frame rate - */ - void correctFramerate(int resolution, double& frame_rate) - { - double max_frame_rate; - std::string reso_str = ""; - switch (resolution) - { - case 0: - max_frame_rate = 15; - reso_str = "2K"; - break; - case 1: - max_frame_rate = 30; - reso_str = "FHD"; - break; - case 2: - max_frame_rate = 60; - reso_str = "HD"; - break; - case 3: - max_frame_rate = 100; - reso_str = "VGA"; - break; - default: - ROS_FATAL("Unknow resolution passed"); - return; - } - if (frame_rate > max_frame_rate) - ROS_WARN("frame_rate(%fHz) too high for resolution(%s), downgraded to %fHz", frame_rate, reso_str.c_str(), - max_frame_rate); - frame_rate = max_frame_rate; - } + fsSettings["LEFT.R"] >> R_l; + fsSettings["RIGHT.R"] >> R_r; + + fsSettings["LEFT.D"] >> D_l; + fsSettings["RIGHT.D"] >> D_r; + + int cols_l=672; + int rows_l=376; + int cols_r=672; + int rows_r=376; + + if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || + rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) + { + std::cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << "/n"; + return ; + } + + cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l); + cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r); + + } + + + /** + * @brief { publish camera info } + * + * @param[in] pub_cam_info The pub camera information + * @param[in] cam_info_msg The camera information message + * @param[in] now The now + */ + void publishCamInfo(const ros::Publisher& pub_cam_info, sensor_msgs::CameraInfo& cam_info_msg, ros::Time now) { + cam_info_msg.header.stamp = now; + pub_cam_info.publish(cam_info_msg); + } + + /** + * @brief { publish image } + * + * @param[in] img The image + * @param img_pub The image pub + * @param[in] img_frame_id The image frame identifier + * @param[in] t { parameter_description } + */ + void publishImage(cv::Mat img, image_transport::Publisher &img_pub, std::string img_frame_id, ros::Time t) { + cv_bridge::CvImage cv_image; + cv_image.image = img; + cv_image.encoding = sensor_msgs::image_encodings::BGR8; + cv_image.header.frame_id = img_frame_id; + cv_image.header.stamp = t; + img_pub.publish(cv_image.toImageMsg()); + } private: - int device_id_, resolution_; - double frame_rate_; - bool show_image_, use_zed_config_; - double width_, height_; - std::string left_frame_id_, right_frame_id_; - std::string config_file_location_; - std::string encoding_; + int resolution_; + double frame_rate_; + bool show_image_, load_zed_config_, rectify_image_; + double width_, height_; + std::string left_frame_id_, right_frame_id_; + std::string config_file_location_; + std::string undistort_config_file_location_; + cv::Mat M1l, M2l, M1r,M2r; }; + } -int main(int argc, char** argv) -{ - try - { - ros::init(argc, argv, "zed_camera"); - arti::ZedCameraROS zed_ros; - return EXIT_SUCCESS; - } - catch (std::runtime_error& e) - { - ros::shutdown(); - return EXIT_FAILURE; - } + +int main(int argc, char **argv) { + try { + ros::init(argc, argv, "zed_camera"); + arti::ZedCameraROS zed_ros; + return EXIT_SUCCESS; + } + catch(std::runtime_error& e) { + ros::shutdown(); + return EXIT_FAILURE; + } } From 34ec339f656b925a6a6d677870870f75e6e3f3cb Mon Sep 17 00:00:00 2001 From: ebimor Date: Tue, 24 Apr 2018 13:10:42 -0400 Subject: [PATCH 2/6] config file --- config/SN1267.conf | 78 ------------------------------------------- config/SN16988.conf | 81 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 81 insertions(+), 78 deletions(-) delete mode 100644 config/SN1267.conf create mode 100644 config/SN16988.conf diff --git a/config/SN1267.conf b/config/SN1267.conf deleted file mode 100644 index 1df0f47..0000000 --- a/config/SN1267.conf +++ /dev/null @@ -1,78 +0,0 @@ -[LEFT_CAM_2K] -cx=1164.13 -cy=574.379 -fx=1400.09 -fy=1400.09 -k1=-0.171543 -k2=0.0240596 - -[LEFT_CAM_FHD] -cx=1020.13 -cy=493.379 -fx=1400.09 -fy=1400.09 -k1=-0.171543 -k2=0.0240596 - -[LEFT_CAM_HD] -cx=668.567 -cy=335.19 -fx=700.045 -fy=700.045 -k1=-0.171543 -k2=0.0240596 - -[LEFT_CAM_VGA] -cx=349.784 -cy=175.095 -fx=350.023 -fy=350.023 -k1=-0.171543 -k2=0.0240596 - -[RIGHT_CAM_2K] -cx=1061.5 -cy=630.943 -fx=1400.05 -fy=1400.05 -k1=-0.171688 -k2=0.0240001 - -[RIGHT_CAM_FHD] -cx=917.499 -cy=549.943 -fx=1400.05 -fy=1400.05 -k1=-0.171688 -k2=0.0240001 - -[RIGHT_CAM_HD] -cx=617.249 -cy=363.471 -fx=700.025 -fy=700.025 -k1=-0.171688 -k2=0.0240001 - -[RIGHT_CAM_VGA] -cx=324.125 -cy=189.236 -fx=350.013 -fy=350.013 -k1=-0.171688 -k2=0.0240001 - -[STEREO] -BaseLine=120 -CV_2K=0.0223798 -CV_FHD=0.0223798 -CV_HD=0.0223798 -CV_VGA=0.0223798 -RX_2K=0.00388714 -RX_FHD=0.00388714 -RX_HD=0.00388714 -RX_VGA=0.00388714 -RZ_2K=-0.00139603 -RZ_FHD=-0.00139603 -RZ_HD=-0.00139603 -RZ_VGA=-0.00139603 diff --git a/config/SN16988.conf b/config/SN16988.conf new file mode 100644 index 0000000..56ef33c --- /dev/null +++ b/config/SN16988.conf @@ -0,0 +1,81 @@ +[LEFT_CAM_2K] +fx=1399.19 +fy=1399.19 +cx=1120.12 +cy=605.935 +k1=-0.175198 +k2=0.0283066 + +[RIGHT_CAM_2K] +fx=1397.86 +fy=1397.86 +cx=1088.53 +cy=619.88 +k1=-0.169327 +k2=0.0242271 + +[LEFT_CAM_FHD] +fx=1399.19 +fy=1399.19 +cx=976.118 +cy=524.935 +k1=-0.175198 +k2=0.0283066 + +[RIGHT_CAM_FHD] +fx=1397.86 +fy=1397.86 +cx=944.533 +cy=538.88 +k1=-0.169327 +k2=0.0242271 + +[LEFT_CAM_HD] +fx=699.594 +fy=699.594 +cx=646.559 +cy=350.967 +k1=-0.175198 +k2=0.0283066 + +[RIGHT_CAM_HD] +fx=698.93 +fy=698.93 +cx=630.767 +cy=357.94 +k1=-0.169327 +k2=0.0242271 + +[LEFT_CAM_VGA] +fx=349.797 +fy=349.797 +cx=338.78 +cy=182.984 +k1=-0.175198 +k2=0.0283066 + +[RIGHT_CAM_VGA] +fx=349.465 +fy=349.465 +cx=330.883 +cy=186.47 +k1=-0.169327 +k2=0.0242271 + +[STEREO] +Baseline=120 +CV_2K=0.0151461 +CV_FHD=0.0151461 +CV_HD=0.0151461 +CV_VGA=0.0151461 +RX_2K=-0.00494921 +RX_FHD=-0.00494921 +RX_HD=-0.00494921 +RX_VGA=-0.00494921 +RZ_2K=-0.00136568 +RZ_FHD=-0.00136568 +RZ_HD=-0.00136568 +RZ_VGA=-0.00136568 + + + From 18a5c266ac219dfe5ab9d2475d47f7a0ec0a16bf Mon Sep 17 00:00:00 2001 From: Ebrahim Moradi Shahrivar Date: Thu, 10 May 2018 11:10:21 -0400 Subject: [PATCH 3/6] automatic rectification --- .clang-format | 0 .travis.yml | 0 CMakeLists.txt | 4 +- LICENSE | 0 README.md | 0 catkin.options | 0 config/ORB_SLAM_ZED.yaml | 0 config/SN16988.conf | 0 config/left.yaml | 0 config/right.yaml | 0 config/roscalib | 75 ++++++++++++++++++++++ dependencies.rosinstall | 0 launch/camera_calibration.launch | 0 launch/depth.launch | 0 launch/image_proc.launch | 0 launch/zed_cpu_ros.launch | 0 package.xml | 0 src/zed_cpu_ros.cpp | 106 +++++++++++++++++++++++++------ 18 files changed, 162 insertions(+), 23 deletions(-) mode change 100644 => 100755 .clang-format mode change 100644 => 100755 .travis.yml mode change 100644 => 100755 CMakeLists.txt mode change 100644 => 100755 LICENSE mode change 100644 => 100755 README.md mode change 100644 => 100755 catkin.options mode change 100644 => 100755 config/ORB_SLAM_ZED.yaml mode change 100644 => 100755 config/SN16988.conf mode change 100644 => 100755 config/left.yaml mode change 100644 => 100755 config/right.yaml create mode 100644 config/roscalib mode change 100644 => 100755 dependencies.rosinstall mode change 100644 => 100755 launch/camera_calibration.launch mode change 100644 => 100755 launch/depth.launch mode change 100644 => 100755 launch/image_proc.launch mode change 100644 => 100755 launch/zed_cpu_ros.launch mode change 100644 => 100755 package.xml diff --git a/.clang-format b/.clang-format old mode 100644 new mode 100755 diff --git a/.travis.yml b/.travis.yml old mode 100644 new mode 100755 diff --git a/CMakeLists.txt b/CMakeLists.txt old mode 100644 new mode 100755 index 8d33b9f..843f6aa --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,14 +12,14 @@ find_package(catkin REQUIRED ) # set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}") -# find_package(Eigen3 REQUIRED) +find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED) find_package(OpenCV REQUIRED) include_directories( include ${catkin_INCLUDE_DIRS} - ${EIGEN_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) diff --git a/LICENSE b/LICENSE old mode 100644 new mode 100755 diff --git a/README.md b/README.md old mode 100644 new mode 100755 diff --git a/catkin.options b/catkin.options old mode 100644 new mode 100755 diff --git a/config/ORB_SLAM_ZED.yaml b/config/ORB_SLAM_ZED.yaml old mode 100644 new mode 100755 diff --git a/config/SN16988.conf b/config/SN16988.conf old mode 100644 new mode 100755 diff --git a/config/left.yaml b/config/left.yaml old mode 100644 new mode 100755 diff --git a/config/right.yaml b/config/right.yaml old mode 100644 new mode 100755 diff --git a/config/roscalib b/config/roscalib new file mode 100644 index 0000000..c531237 --- /dev/null +++ b/config/roscalib @@ -0,0 +1,75 @@ + +Left: +('D = ', [-0.17209557748433973, 0.019845104434456348, 0.00027261236865564414, 0.0016244601754901989, 0.0]) +('K = ', [346.9886945006349, 0.0, 342.85535647927907, 0.0, 348.4307489217302, 188.1122835603919, 0.0, 0.0, 1.0]) +('R = ', [0.9999425796061927, 0.0029751801025626403, 0.010294940207228378, -0.0030202762948984122, 0.999985900674699, 0.004367651876408269, -0.010281800504559827, -0.0043984946479828, 0.9999374669564173]) +('P = ', [447.6473712801969, 0.0, 274.16839599609375, 0.0, 0.0, 447.6473712801969, 252.97887706756592, 0.0, 0.0, 0.0, 1.0, 0.0]) + +Right: +('D = ', [-0.15663493579274437, -0.0040409990254712804, 0.0004213587763997123, 0.0007789422114619799, 0.0]) +('K = ', [348.99129838873466, 0.0, 333.3911990483944, 0.0, 349.33097992390356, 190.2016195118505, 0.0, 0.0, 1.0]) +('R = ', [0.999910992826379, 0.000784361000948321, 0.013318828881901066, -0.000725973926201859, 0.9999901088594528, -0.00438806849514147, -0.013322138973289746, 0.004378008803071259, 0.9999016719918498]) +('P = ', [447.6473712801969, 0.0, 274.16839599609375, -47.97524256627728, 0.0, 447.6473712801969, 252.97887706756592, 0.0, 0.0, 0.0, 1.0, 0.0]) +('self.T ', [-0.1071624128794584, -8.406149950671951e-05, -0.0014274048889883432]) +('self.R ', [0.9999927457604559, 0.0023075489567089486, -0.003030452817080847, -0.0022809442714145263, 0.9999590866405643, 0.008753418657825964, 0.003050527773166973, -0.008746442864437284, 0.9999570960883893]) +# oST version 5.0 parameters + + +[image] + +width +672 + +height +376 + +[narrow_stereo/left] + +camera matrix +346.988695 0.000000 342.855356 +0.000000 348.430749 188.112284 +0.000000 0.000000 1.000000 + +distortion +-0.172096 0.019845 0.000273 0.001624 0.000000 + +rectification +0.999943 0.002975 0.010295 +-0.003020 0.999986 0.004368 +-0.010282 -0.004398 0.999937 + +projection +447.647371 0.000000 274.168396 0.000000 +0.000000 447.647371 252.978877 0.000000 +0.000000 0.000000 1.000000 0.000000 + +# oST version 5.0 parameters + + +[image] + +width +672 + +height +376 + +[narrow_stereo/right] + +camera matrix +348.991298 0.000000 333.391199 +0.000000 349.330980 190.201620 +0.000000 0.000000 1.000000 + +distortion +-0.156635 -0.004041 0.000421 0.000779 0.000000 + +rectification +0.999911 0.000784 0.013319 +-0.000726 0.999990 -0.004388 +-0.013322 0.004378 0.999902 + +projection +447.647371 0.000000 274.168396 -47.975243 +0.000000 447.647371 252.978877 0.000000 +0.000000 0.000000 1.000000 0.000000 diff --git a/dependencies.rosinstall b/dependencies.rosinstall old mode 100644 new mode 100755 diff --git a/launch/camera_calibration.launch b/launch/camera_calibration.launch old mode 100644 new mode 100755 diff --git a/launch/depth.launch b/launch/depth.launch old mode 100644 new mode 100755 diff --git a/launch/image_proc.launch b/launch/image_proc.launch old mode 100644 new mode 100755 diff --git a/launch/zed_cpu_ros.launch b/launch/zed_cpu_ros.launch old mode 100644 new mode 100755 diff --git a/package.xml b/package.xml old mode 100644 new mode 100755 diff --git a/src/zed_cpu_ros.cpp b/src/zed_cpu_ros.cpp index 630e7bc..6bbdfc2 100644 --- a/src/zed_cpu_ros.cpp +++ b/src/zed_cpu_ros.cpp @@ -32,11 +32,16 @@ class StereoCamera * @param[in] frame_rate The frame rate */ StereoCamera(int resolution, double frame_rate): frame_rate_(30.0) { + std::cout<<"Setting resolution4"<<'\n'; + + camera_ = new cv::VideoCapture(0); + std::cout<<"Setting resolution5"<<'\n'; - camera_ = new cv::VideoCapture(1); cv::Mat raw; cv::Mat left_image; cv::Mat right_image; + std::cout<<"Setting resolution6"<<'\n'; + setResolution(resolution); // // this function doesn't work very well in current Opencv 2.4, so, just use ROS to control frame rate. // setFrameRate(frame_rate); @@ -57,18 +62,24 @@ class StereoCamera */ void setResolution(int type) { + std::cout<<"Setting resolution1"<<'\n'; + if (type == 0) { width_ = 4416; height_ = 1242;} // 2K if (type == 1) { width_ = 3840; height_ = 1080;} // FHD if (type == 2) { width_ = 2560; height_ = 720;} // HD if (type == 3) { width_ = 1344; height_ = 376;} // VGA + std::cout<<"Setting resolution2"<<'\n'; + camera_->set(WIDTH_ID, width_); camera_->set(HEIGHT_ID, height_); + std::cout<<"Setting resolution3"<<'\n'; // make sure that the number set are right from the hardware width_ = camera_->get(WIDTH_ID); height_ = camera_->get(HEIGHT_ID); + } /** @@ -103,11 +114,13 @@ class StereoCamera return false; } } + int width_; + int height_; private: + cv::VideoCapture* camera_; - int width_; - int height_; + double frame_rate_; bool cv_three_; }; @@ -124,7 +137,9 @@ class ZedCameraROS { * @param[in] resolution The resolution * @param[in] frame_rate The frame rate */ - ZedCameraROS() { + + + ZedCameraROS(){ ros::NodeHandle nh; ros::NodeHandle private_nh("~"); // get ros param @@ -142,6 +157,11 @@ class ZedCameraROS { StereoCamera zed(resolution_, frame_rate_); ROS_INFO("Initialized the camera"); + + WIDTH=zed.width_; + HEIGHT=zed.height_; + + // setup publisher stuff image_transport::ImageTransport it(nh); image_transport::Publisher left_image_pub = it.advertise("left/image_raw", 1); @@ -150,6 +170,9 @@ class ZedCameraROS { image_transport::Publisher rec_left_image_pub = it.advertise("left/image_rectified", 1); image_transport::Publisher rec_right_image_pub = it.advertise("right/image_rectified", 1); + image_transport::Publisher rec_whole_image_pub = it.advertise("wholeRecImage", 1); + image_transport::Publisher raw_whole_image_pub = it.advertise("wholeRawImage", 1); + ros::Publisher left_cam_info_pub = nh.advertise("left/camera_info", 1); ros::Publisher right_cam_info_pub = nh.advertise("right/camera_info", 1); @@ -190,7 +213,7 @@ class ZedCameraROS { ROS_INFO("Got camera calibration files"); // loop to publish images; - cv::Mat left_image, right_image; + cv::Mat left_image, right_image, wholeRawImage; ros::Rate r(frame_rate_); while (nh.ok()) { @@ -205,10 +228,13 @@ class ZedCameraROS { cv::imshow("right", right_image); } + hconcat(left_image,right_image,wholeRawImage); + if(rectify_image_){ - cv::Mat imLeftRec, imRightRec; + cv::Mat imLeftRec, imRightRec, wholeRecImage; cv::remap(left_image,imLeftRec,M1l,M2l,cv::INTER_LINEAR); cv::remap(right_image,imRightRec,M1r,M2r,cv::INTER_LINEAR); + hconcat(imLeftRec,imRightRec,wholeRecImage); cv::imshow("left_rectified", imLeftRec); if (rec_left_image_pub.getNumSubscribers() > 0) { publishImage(imLeftRec, rec_left_image_pub, "rec_left_frame", now); @@ -216,6 +242,13 @@ class ZedCameraROS { if (rec_right_image_pub.getNumSubscribers() > 0) { publishImage(imRightRec, rec_right_image_pub, "rec_right_frame", now); } + if (rec_whole_image_pub.getNumSubscribers() > 0) { + publishImage(wholeRecImage, rec_whole_image_pub, "rec_whole_frame", now); + } + } + + if (raw_whole_image_pub.getNumSubscribers() > 0) { + publishImage(wholeRawImage, raw_whole_image_pub, "raw_whole_frame", now); } if (left_image_pub.getNumSubscribers() > 0) { @@ -292,6 +325,12 @@ class ZedCameraROS { // assume zeros, maybe not right double p1 = 0, p2 = 0, k3 = 0; + left_info.height=HEIGHT; + left_info.width=WIDTH; + + right_info.height=HEIGHT; + right_info.width=WIDTH; + left_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; right_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; @@ -304,6 +343,9 @@ class ZedCameraROS { left_info.D[3] = p1; left_info.D[4] = p2; + D_l=(cv::Mat1d(1,5) << l_k1, l_k2, k3, p1, p2); + D_r=(cv::Mat1d(1,5) << r_k1, r_k2, k3, p1, p2); + right_info.D.resize(5); right_info.D[0] = r_k1; right_info.D[1] = r_k2; @@ -329,6 +371,12 @@ class ZedCameraROS { right_info.K[5] = r_cy; right_info.K[8] = 1.0; + K_l = (cv::Mat1d(3, 3) << l_fx, 0, l_cx, 0, l_fy, l_cy, 0, 0, 1); + K_r = (cv::Mat1d(3, 3) << r_fx, 0, r_cx, 0, r_fy, r_cy, 0, 0, 1); + + //std::cout<<"left focal length: "<(0,0)<<'\n'; + + // rectification matrix // Rl = R_rect, R_r = R * R_rect // since R is identity, Rl = Rr; @@ -342,7 +390,13 @@ class ZedCameraROS { for (it = rmat.begin(); it != rmat.end(); ++it, id++){ left_info.R[id] = *it; right_info.R[id] = *it; + } + R_r=rmat; + R_l=rmat; + //std::cout<<"left focal length: "<(0,0)<<'\n'; + + // Projection/camera matrix // [fx' 0 cx' Tx] @@ -363,6 +417,9 @@ class ZedCameraROS { right_info.P[6] = r_cy; right_info.P[10] = 1.0; + P_l=(cv::Mat1d(3, 4) << l_fx, 0, l_cx, 0, 0, l_fy, l_cy, 0, 0, 0, 1, 0); + P_r=(cv::Mat1d(3, 4) << r_fx, 0, r_cx, -1 * l_fx * baseline, 0, r_fy, r_cy, 0, 0, 0, 1, 0); + left_info.width = right_info.width = width_; left_info.height = right_info.height = height_; @@ -383,24 +440,27 @@ class ZedCameraROS { return ; } - cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; - fsSettings["LEFT.K"] >> K_l; - fsSettings["RIGHT.K"] >> K_r; + cv::Mat UK_l, UK_r, UP_l, UP_r, UR_l, UR_r, UD_l, UD_r; + + fsSettings["LEFT.K"] >> UK_l; + fsSettings["RIGHT.K"] >> UK_r; - fsSettings["LEFT.P"] >> P_l; - fsSettings["RIGHT.P"] >> P_r; + fsSettings["LEFT.P"] >> UP_l; + fsSettings["RIGHT.P"] >> UP_r; - fsSettings["LEFT.R"] >> R_l; - fsSettings["RIGHT.R"] >> R_r; + fsSettings["LEFT.R"] >> UR_l; + fsSettings["RIGHT.R"] >> UR_r; - fsSettings["LEFT.D"] >> D_l; - fsSettings["RIGHT.D"] >> D_r; + fsSettings["LEFT.D"] >> UD_l; + fsSettings["RIGHT.D"] >> UD_r; - int cols_l=672; - int rows_l=376; - int cols_r=672; - int rows_r=376; + int cols_l=WIDTH; + int rows_l=HEIGHT; + int cols_r=WIDTH; + int rows_r=HEIGHT; + + if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) { @@ -408,8 +468,10 @@ class ZedCameraROS { return ; } - cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l); - cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r); + std::cout<<"WIDTH is: "< Date: Thu, 10 May 2018 11:21:24 -0400 Subject: [PATCH 4/6] removedFile --- config/ORB_SLAM_ZED.yaml | 116 --------------------------------------- 1 file changed, 116 deletions(-) delete mode 100755 config/ORB_SLAM_ZED.yaml diff --git a/config/ORB_SLAM_ZED.yaml b/config/ORB_SLAM_ZED.yaml deleted file mode 100755 index c22dd65..0000000 --- a/config/ORB_SLAM_ZED.yaml +++ /dev/null @@ -1,116 +0,0 @@ -%YAML:1.0 - -#-------------------------------------------------------------------------------------------- -# Camera Parameters. Adjust them! -#-------------------------------------------------------------------------------------------- - -# Camera calibration and distortion parameters (OpenCV) -Camera.fx: 349.797 -Camera.fy: 349.797 -Camera.cx: 338.78 -Camera.cy: 182.984 - -Camera.k1: -0.175198 -Camera.k2: 0.0283066 -Camera.p1: 0.0 -Camera.p2: 0.0 - -Camera.width: 672 -Camera.height: 376 - -# Camera frames per second -Camera.fps: 30.0 - -# stereo baseline times fx -Camera.bf: 83.95128 - -# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) -Camera.RGB: 1 - -# Close/Far threshold. Baseline times. -ThDepth: 35 - -#-------------------------------------------------------------------------------------------- -# Stereo Rectification. Only if you need to pre-rectify the images. -# Camera.fx, .fy, etc must be the same as in LEFT.P -#-------------------------------------------------------------------------------------------- -LEFT.height: 376 -LEFT.width: 672 -LEFT.D: !!opencv-matrix - rows: 1 - cols: 5 - dt: d - data: [-0.175198, 0.0283066, 0.0, 0.0, 0.0] #[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] -LEFT.K: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [349.797, 0.0, 338.78, 0.0, 349.797, 182.984, 0.0, 0.0, 1.0] -LEFT.R: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [0.9998843677510435, 0.0013281419694759294, 0.015148833816660816, -0.001403101601371256, 0.9999868204001661, 0.0049386568884934, -0.01514207492360671, -0.004959341173477632, 0.9998730531933205] -LEFT.P: !!opencv-matrix - rows: 3 - cols: 4 - dt: d - data: [349.797, 0.0, 338.78, 0.0, 0.0, 349.797, 182.984, 0.0, 0.0, 0.0, 1.0, 0.0] - -RIGHT.height: 376 -RIGHT.width: 672 -RIGHT.D: !!opencv-matrix - rows: 1 - cols: 5 - dt: d - data: [-0.169327, 0.0242271, 0.0, 0.0, 0.0] -RIGHT.K: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [349.465, 0.0, 330.883, 0.0, 349.465, 186.47, 0.0, 0.0, 1.0] -RIGHT.R: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [0.9998843677510435, 0.0013281419694759294, 0.015148833816660816, -0.001403101601371256, 0.9999868204001661, 0.0049386568884934, -0.01514207492360671, -0.004959341173477632, 0.9998730531933205] -RIGHT.P: !!opencv-matrix - rows: 3 - cols: 4 - dt: d - data: [349.465, 0.0, 330.883, -41.97564, 0.0, 349.465, 186.47, 0.0, 0.0, 0.0, 1.0, 0.0] - -#-------------------------------------------------------------------------------------------- -# ORB Parameters -#-------------------------------------------------------------------------------------------- - -# ORB Extractor: Number of features per image -ORBextractor.nFeatures: 1200 - -# ORB Extractor: Scale factor between levels in the scale pyramid -ORBextractor.scaleFactor: 1.2 - -# ORB Extractor: Number of levels in the scale pyramid -ORBextractor.nLevels: 8 - -# ORB Extractor: Fast threshold -# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. -# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST -# You can lower these values if your images have low contrast -ORBextractor.iniThFAST: 20 -ORBextractor.minThFAST: 7 - -#-------------------------------------------------------------------------------------------- -# Viewer Parameters -#-------------------------------------------------------------------------------------------- -Viewer.KeyFrameSize: 0.05 -Viewer.KeyFrameLineWidth: 1 -Viewer.GraphLineWidth: 0.9 -Viewer.PointSize:2 -Viewer.CameraSize: 0.08 -Viewer.CameraLineWidth: 3 -Viewer.ViewpointX: 0 -Viewer.ViewpointY: -0.7 -Viewer.ViewpointZ: -1.8 -Viewer.ViewpointF: 500 - From 08978f14f90827f1155628489377cecd4227a434 Mon Sep 17 00:00:00 2001 From: Ebrahim Moradi Shahrivar Date: Thu, 10 May 2018 11:26:25 -0400 Subject: [PATCH 5/6] removedDebugs --- launch/zed_cpu_ros.launch | 4 ++-- src/zed_cpu_ros.cpp | 45 +++++---------------------------------- 2 files changed, 7 insertions(+), 42 deletions(-) diff --git a/launch/zed_cpu_ros.launch b/launch/zed_cpu_ros.launch index 4538a8e..526a982 100755 --- a/launch/zed_cpu_ros.launch +++ b/launch/zed_cpu_ros.launch @@ -5,10 +5,10 @@ - + - + diff --git a/src/zed_cpu_ros.cpp b/src/zed_cpu_ros.cpp index 6bbdfc2..cd01313 100644 --- a/src/zed_cpu_ros.cpp +++ b/src/zed_cpu_ros.cpp @@ -32,15 +32,12 @@ class StereoCamera * @param[in] frame_rate The frame rate */ StereoCamera(int resolution, double frame_rate): frame_rate_(30.0) { - std::cout<<"Setting resolution4"<<'\n'; camera_ = new cv::VideoCapture(0); - std::cout<<"Setting resolution5"<<'\n'; cv::Mat raw; cv::Mat left_image; cv::Mat right_image; - std::cout<<"Setting resolution6"<<'\n'; setResolution(resolution); // // this function doesn't work very well in current Opencv 2.4, so, just use ROS to control frame rate. @@ -62,19 +59,16 @@ class StereoCamera */ void setResolution(int type) { - std::cout<<"Setting resolution1"<<'\n'; if (type == 0) { width_ = 4416; height_ = 1242;} // 2K if (type == 1) { width_ = 3840; height_ = 1080;} // FHD if (type == 2) { width_ = 2560; height_ = 720;} // HD if (type == 3) { width_ = 1344; height_ = 376;} // VGA - std::cout<<"Setting resolution2"<<'\n'; camera_->set(WIDTH_ID, width_); camera_->set(HEIGHT_ID, height_); - std::cout<<"Setting resolution3"<<'\n'; // make sure that the number set are right from the hardware width_ = camera_->get(WIDTH_ID); height_ = camera_->get(HEIGHT_ID); @@ -146,7 +140,7 @@ class ZedCameraROS { private_nh.param("resolution", resolution_, 1); private_nh.param("frame_rate", frame_rate_, 30.0); private_nh.param("config_file_location", config_file_location_, std::string("~/SN1267.conf")); - private_nh.param("undistort_config_file_location", undistort_config_file_location_, std::string("~/SN1267.conf")); + //private_nh.param("undistort_config_file_location", undistort_config_file_location_, std::string("~/SN1267.conf")); private_nh.param("left_frame_id", left_frame_id_, std::string("left_camera")); private_nh.param("right_frame_id", right_frame_id_, std::string("right_camera")); private_nh.param("show_image", show_image_, false); @@ -185,7 +179,7 @@ class ZedCameraROS { // get camera info from zed try { getZedCameraInfo(config_file_location_, resolution_, left_info, right_info); - getUndistortedMaps(undistort_config_file_location_); + getUndistortedMaps(); } catch (std::runtime_error& e) { ROS_INFO("Can't load camera info"); @@ -431,45 +425,16 @@ class ZedCameraROS { } - void getUndistortedMaps(std::string config_file){ + void getUndistortedMaps(){ - cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); - if(!fsSettings.isOpened()) - { - std::cerr << "ERROR: Wrong path to settings" << "/n"; - return ; - } - - cv::Mat UK_l, UK_r, UP_l, UP_r, UR_l, UR_r, UD_l, UD_r; - - fsSettings["LEFT.K"] >> UK_l; - fsSettings["RIGHT.K"] >> UK_r; - - fsSettings["LEFT.P"] >> UP_l; - fsSettings["RIGHT.P"] >> UP_r; - - fsSettings["LEFT.R"] >> UR_l; - fsSettings["RIGHT.R"] >> UR_r; - - fsSettings["LEFT.D"] >> UD_l; - fsSettings["RIGHT.D"] >> UD_r; - - - int cols_l=WIDTH; - int rows_l=HEIGHT; - int cols_r=WIDTH; - int rows_r=HEIGHT; - - + if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || - rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) + WIDTH==0 || HEIGHT==0) { std::cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << "/n"; return ; } - std::cout<<"WIDTH is: "< Date: Thu, 10 May 2018 11:30:49 -0400 Subject: [PATCH 6/6] removedDebugs --- src/zed_cpu_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/zed_cpu_ros.cpp b/src/zed_cpu_ros.cpp index cd01313..aeb6fc1 100644 --- a/src/zed_cpu_ros.cpp +++ b/src/zed_cpu_ros.cpp @@ -229,7 +229,7 @@ class ZedCameraROS { cv::remap(left_image,imLeftRec,M1l,M2l,cv::INTER_LINEAR); cv::remap(right_image,imRightRec,M1r,M2r,cv::INTER_LINEAR); hconcat(imLeftRec,imRightRec,wholeRecImage); - cv::imshow("left_rectified", imLeftRec); + //cv::imshow("left_rectified", imLeftRec); if (rec_left_image_pub.getNumSubscribers() > 0) { publishImage(imLeftRec, rec_left_image_pub, "rec_left_frame", now); }