长江 5 号双目,ros测试小记
左相机、双目视差、点云小效果
相关软件可以从网盘下载 百度网盘链接:https://pan.baidu.com/s/1UbtPUQCIhYQR4vzhNuXWUQ
提取码:zkhy
https://github.com/SmarterEye/SmarterEye-SDK
开源的 提供全部源代码 客户那边可以在所有平台上编译运行
编译问题
1最新版编译不过,qt版本问题吧。
使用老的版本。SmarterEye-SDK-0.5.1
2qtcreator版本 cmake配置
SmarterEye-SDK-0.5.1/cmake/find_qt.cmake
if (WIN32)
# set stuff for windows
# SET(Qt5_PATH /path/to/your/qt/lib)
else()
# set stuff for other systems
#SET(Qt5_PATH /opt/Qt5.12.2/5.12.2/gcc_64)
SET(Qt5_PATH /opt/Qt5.14.0/5.14.0/gcc_64)
endif()
源码相关修改
去掉opencv窗口
修改坐标系名称
修改时间戳
//
// Created by xtp on 2020/3/9.
//
#include "stereo_publisher.h"
#include "framemonitor.h"
//#include "smarter_eye_sdk/stereocamera.h"
//#include "smarter_eye_sdk/frameid.h"
//#include "smarter_eye_sdk/calibrationparams.h"
//#include "smarter_eye_sdk/rotationmatrix.h"
#include "stereocamera.h"
#include "frameid.h"
#include "calibrationparams.h"
#include "rotationmatrix.h"
#include "cv_bridge/cv_bridge.h"
#include "std_msgs/String.h"
#include "sensor_msgs/Imu.h"
#include <cmath>
static const std::string kLeftGrayTopic("/zkhy_stereo/left/gray");
static const std::string kLeftColorTopic("/zkhy_stereo/left/color");
static const std::string kRigthGrayTopic("/zkhy_stereo/right/gray");
static const std::string kRigthColorTopic("/zkhy_stereo/right/color");
static const std::string kDisparityTopic("/zkhy_stereo/disparity");
static const std::string kImuTopic("/zkhy_stereo/imu");
static const std::string kPointCloudTopic("/zkhy_stereo/points");
static const std::string kCameraParamsService("/zkhy_stereo/get_camera_params");
static const std::string kRotationMatrixService("/zkhy_stereo/get_rotation_matrix");
static const std::string kFrameRateService("/zkhy_stereo/set_frame_rate");
StereoPublisher::StereoPublisher()
: it_(node_handler_),
stereo_camera_(StereoCamera::connect("192.168.1.251")),
frame_monitor_(new FrameMonitor)
{
bool isConnected = false;
// frame rate
float rate = 0.0;
while (!isConnected) {
printf("connecting...\n");
isConnected = stereo_camera_->isConnected();
std::this_thread::sleep_for(std::chrono::seconds(1));
}
if(stereo_camera_->getFrameRate(rate))
{
std::cout <<"frame rate : "<<rate<<std::endl;
}
else
{
std::cout << "get frame rate failed" <<std::endl;
}
left_gray_pub_ = it_.advertise(kLeftGrayTopic, 1);
left_color_pub_ = it_.advertise(kLeftColorTopic, 1);
right_gray_pub_ = it_.advertise(kRigthGrayTopic, 1);
right_color_pub_ = it_.advertise(kRigthColorTopic, 1);
disparity_pub_ = it_.advertise(kDisparityTopic, 1);
imu_pub_ = node_handler_.advertise<sensor_msgs::Imu>(kImuTopic, 100);
points_pub_ = node_handler_.advertise<sensor_msgs::PointCloud2>(kPointCloudTopic, 1);
camera_params_server_ = node_handler_.advertiseService(kCameraParamsService, &StereoPublisher::onCameraParamsRequest, this);
rotation_matrix_server_ = node_handler_.advertiseService(kRotationMatrixService, &StereoPublisher::onRotationMatrixRequest, this);
frame_rate_server_ = node_handler_.advertiseService(kFrameRateService, &StereoPublisher::onFrameRateSetRequest, this);
stereo_camera_->requestFrame(frame_monitor_.get(), FrameId::CalibLeftCamera | FrameId::CalibRightCamera | FrameId::Disparity);
stereo_camera_->enableMotionData(true);
StereoCalibrationParameters params {};
stereo_camera_->requestStereoCameraParameters(params);
frame_monitor_->setStereoCalibParams(params);
frame_monitor_->setFrameCallback(std::bind(&StereoPublisher::publishFrameCallback, this,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
frame_monitor_->setMotionDataCallback(std::bind(&StereoPublisher::publishImuCallback, this, std::placeholders::_1));
frame_monitor_->setPointCloudCallback(std::bind(&StereoPublisher::publishPointsCallback, this, std::placeholders::_1));
}
StereoPublisher::~StereoPublisher()
{
stereo_camera_->disconnectFromServer();
}
bool StereoPublisher::onCameraParamsRequest(zkhy_stereo_d::CameraParams::Request &req,
zkhy_stereo_d::CameraParams::Response &resp)
{
StereoCalibrationParameters params {};
bool ok = stereo_camera_->requestStereoCameraParameters(params);
if (ok) {
resp.focus = params.focus;
resp.cx = params.cx;
resp.cy = params.cy;
resp.RRoll = params.RRoll;
resp.RPitch = params.RPitch;
resp.RYaw = params.RYaw;
resp.Tx = params.Tx;
resp.Ty = params.Ty;
resp.Tz = params.Tz;
} else {
std::cout << "Stereo camera params request failed from ADAS.";
}
return ok;
}
bool StereoPublisher::onRotationMatrixRequest(zkhy_stereo_d::RotationMatrix::Request &req,
zkhy_stereo_d::RotationMatrix::Response &resp)
{
RotationMatrix matrix {};
bool ok = stereo_camera_->requestRotationMatrix(matrix);
if (ok) {
for (size_t i = 0; i < resp.real3DToImage.size(); i++) {
resp.real3DToImage[i] = matrix.real3DToImage[i];
}
for (size_t j = 0; j < resp.imageToReal3D.size(); j++) {
resp.imageToReal3D[j] = matrix.imageToReal3D[j];
}
} else {
std::cout << "Rotation matrix request failed from ADAS.";
}
return ok;
}
bool StereoPublisher::onFrameRateSetRequest(zkhy_stereo_d::FrameRate::Request &req,zkhy_stereo_d::FrameRate::Response &resp)
{
float rate = req.rate;
std::cout << " request rate :"<<rate<<std::endl;
bool ok = stereo_camera_->setFrameRate(rate);
if(ok)
{
std::cout << " set frame rate done"<<std::endl;
}
else
{
std::cout << " set frame rate failed"<<std::endl;
}
return ok;
}
void StereoPublisher::publishFrameCallback(int frameId, int64_t timestamp, const cv::Mat &img_mat)
{
bool is_color = (img_mat.type() == CV_8UC3);
std::string encoding = is_color ? "rgb8" : "mono8";
sensor_msgs::ImagePtr img_msg;
if (frameId == FrameId::CalibLeftCamera || frameId == FrameId::CalibRightCamera) {
img_msg = cv_bridge::CvImage(std_msgs::Header(), encoding, img_mat).toImageMsg();
//img_msg->header.stamp = ros::Time(timestamp / 1000.0);
img_msg->header.stamp = ros::Time::now();
if (frameId == FrameId::CalibLeftCamera) {
if (is_color) {
img_msg->header.frame_id = "zkhy_left_color_frame";
left_color_pub_.publish(img_msg);
} else {
img_msg->header.frame_id = "zkhy_left_gray_frame";
left_gray_pub_.publish(img_msg);
}
}
if (frameId == FrameId::CalibRightCamera) {
if (is_color) {
img_msg->header.frame_id = "zkhy_right_color_frame";
right_color_pub_.publish(img_msg);
} else {
img_msg->header.frame_id = "zkhy_right_gray_frame";
right_gray_pub_.publish(img_msg);
}
}
}
if (frameId == FrameId::Disparity) {
img_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", img_mat).toImageMsg();
img_msg->header.frame_id = "zkhy_disparity_frame";
disparity_pub_.publish(img_msg);
}
}
void StereoPublisher::publishImuCallback(const MotionData *motionData)
{
sensor_msgs::Imu imu_msg;
//imu_msg.header.stamp = ros::Time(motionData->timestamp / 1000.0);
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = "zkhy_imu_frame";
imu_msg.linear_acceleration.x = motionData->accelX;
imu_msg.linear_acceleration.y = motionData->accelY;
imu_msg.linear_acceleration.z = motionData->accelZ;
imu_msg.angular_velocity.x = motionData->gyroX * M_PI / 180;
imu_msg.angular_velocity.y = motionData->gyroY * M_PI / 180;
imu_msg.angular_velocity.z = motionData->gyroZ * M_PI / 180;
imu_pub_.publish(imu_msg);
}
void StereoPublisher::publishPointsCallback(sensor_msgs::PointCloud2::Ptr cloud)
{
cloud->header.frame_id = "stereo_left";
points_pub_.publish(cloud);
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "stereo_publisher_d");
StereoPublisher stereo_publisher;
ros::spin();
return 0;
}
#include "pointcloudgenerator.h"
#include <iostream>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
//#include "smarter_eye_sdk/satpext.h"
//#include "smarter_eye_sdk/disparityconvertor.h"
#include "satpext.h"
#include "disparityconvertor.h"
PointCloudGenerator::PointCloudGenerator()
: width_(0),
height_(0),
bit_num_(0),
running_(false),
frame_ready_(false),
disparity_raw_data_(nullptr)
{
}
PointCloudGenerator::~PointCloudGenerator()
{
stop();
}
void PointCloudGenerator::init(int width, int height, int dataSize, int bitNum)
{
width_ = width;
height_ = height;
bit_num_ = bitNum;
disparity_raw_data_.reset((RawImageFrame*)new char[sizeof(RawImageFrame) + dataSize]);
start();
}
void PointCloudGenerator::push(const RawImageFrame *rawFrame)
{
if (!prepared()) return;
if (!running_ || frame_ready_) return;
{
std::lock_guard<std::mutex> lock(mutex_);
if (frame_ready_) return;
memcpy(disparity_raw_data_.get(), rawFrame, sizeof(RawImageFrame) + rawFrame->dataSize);
frame_ready_ = true;
}
condition_.notify_one();
}
void PointCloudGenerator::start()
{
if (!prepared()) return;
if (running_) return;
{
std::lock_guard<std::mutex> _(mutex_);
if (running_) return;
running_ = true;
}
thread_ = std::thread(&PointCloudGenerator::run, this);
}
void PointCloudGenerator::stop()
{
if (!running_) return;
{
std::lock_guard<std::mutex> _(mutex_);
if (!running_) return;
running_ = false;
frame_ready_ = true;
}
condition_.notify_one();
if (thread_.joinable()) {
thread_.join();
}
}
void PointCloudGenerator::run()
{
while (running_) {
{
std::unique_lock<std::mutex> lock(mutex_);
condition_.wait(lock, [this]{ return frame_ready_; });
if (!running_) break;
}
sensor_msgs::PointCloud2::Ptr pc(new sensor_msgs::PointCloud2);
//pc->header.stamp = ros::Time(disparity_raw_data_->time / 1000.0);
pc->header.stamp = ros::Time::now();
pc->width = width_;
pc->height = height_;
sensor_msgs::PointCloud2Modifier modifier(*pc);
modifier.setPointCloud2Fields(3,
"x", 1, sensor_msgs::PointField::FLOAT32,
"y", 1, sensor_msgs::PointField::FLOAT32,
"z", 1, sensor_msgs::PointField::FLOAT32);
sensor_msgs::PointCloud2Iterator<float> iter_x(*pc, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(*pc, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(*pc, "z");
for (int posY = 0; posY < height_; posY++) {
for (int posX = 0; posX < width_; posX++) {
float x, y, z;
DisparityConvertor::getPointXYZDistance(disparity_raw_data_->image, width_, height_, bit_num_,
calib_params_.Tx, calib_params_.focus, calib_params_.cx, calib_params_.cy,
posX, posY, x, y, z);
if ((fabs(x) > 200000.0f)||(fabs(y) > 200000.0f)||(fabs(z) > 200000.0f)) {
x = 0.0f;
y = 0.0f;
z = 0.0f;
}
*iter_x = x / 1000.f;
*iter_y = y / 1000.f;
*iter_z = z / 1000.f;
++iter_x; ++iter_y; ++iter_z;
}
}
publish_callback_(pc);
{
std::lock_guard<std::mutex> _(mutex_);
frame_ready_ = false;
}
}
}
有关cameraInfo 官方ros包中没有
使用提供的service转换一下试试
#! /usr/bin/env python
# -*-coding:utf-8 -*-
import rospy
from zkhy_stereo_d.srv import CameraParams,FrameRate,RotationMatrix
#zkhy_stereo_d/CameraParams
#static const std::string kCameraParamsService("/zkhy_stereo/get_camera_params");
'''
---
float64 focus
float64 cx
float64 cy
float64 RRoll
float64 RPitch
float64 RYaw
float64 Tx
float64 Ty
float64 Tz
'''
#static const std::string kRotationMatrixService("/zkhy_stereo/get_rotation_matrix");
'''
---
float32[12] real3DToImage
float32[9] imageToReal3D
'''
#static const std::string kFrameRateService("/zkhy_stereo/set_frame_rate");
'''
float64 rate
---
'''
from sensor_msgs.msg import CameraInfo
from sensor_msgs.msg import Image
class ZKHYCamInfoNode():
def __init__(self):
rospy.init_node("node_zzztestsrv")
rospy.on_shutdown(self.shutdown)
self.rate_hz = rospy.get_param("~rate", 10)
self.rate = rospy.Rate(self.rate_hz)
#
self._srv_camera_params = rospy.get_param("~get_camera_params", "/zkhy_stereo/get_camera_params")
self._srv_rotation_matrix = rospy.get_param("~get_rotation_matrix", "/zkhy_stereo/get_rotation_matrix")
self.threshold_s = rospy.get_param("~threshold", 2)
self._frame = rospy.get_param("~frame_id", "stereo_left")
self._height = rospy.get_param("~img_height", 720)
self._width = rospy.get_param("~img_width", 1280)
self._f = rospy.get_param("~f", 924)
self._fx = self._f
self._fy = self._f
self._cx = self._width/2.0
self._cy = self._height/2.0
self._p = [self._fx, 0.0, self._cx, 0.0, 0.0, self._fx, self._cy, 0.0, 0.0, 0.0, 1.0, 0.0]
self._pub_info = rospy.Publisher('~camera_info', CameraInfo, queue_size=5)
def refresh(self):
try:
client1 = rospy.ServiceProxy(self._srv_camera_params,CameraParams)
client3 = rospy.ServiceProxy(self._srv_rotation_matrix,RotationMatrix)
response = client1.call()
print("/zkhy_stereo/get_camera_params callok")
print(response)
self._f=response.focus
self._fx = self._f
self._fy = self._f
self._cx=response.cx
self._cy=response.cy
response = client3.call()
print("/zkhy_stereo/get_rotation_matrix callok")
print(response)
self._p=response.real3DToImage
#print(response.real3DToImage)
except:
pass
print "call Service err "
def pubcaminfomsg(self):
""":obj:`sensor_msgs.CamerInfo` : Returns ROS CamerInfo msg
msg_header = Header()
msg_header.frame_id = self._frame
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
"""
msg = CameraInfo()
#msg.header = msg_header
msg.header.frame_id = self._frame
msg.height = self._height
msg.width = self._width
# The distortion model used. Supported models are listed in
# sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
# simple model of radial and tangential distortion - is sufficient.
msg.distortion_model = 'plumb_bob'
# The distortion parameters, size depending on the distortion model.
# For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
# Intrinsic camera matrix for the raw (distorted) images.
# [fx 0 cx]
# K = [ 0 fy cy]
# [ 0 0 1]
# Projects 3D points in the camera coordinate frame to 2D pixel
# coordinates using the focal lengths (fx, fy) and principal point
# (cx, cy).
msg.K = [self._fx, 0.0, self._cx, 0.0, self._fy, self._cy, 0.0, 0.0, 1.0]
# Rectification matrix (stereo cameras only)
# A rotation matrix aligning the camera coordinate system to the ideal
# stereo image plane so that epipolar lines in both stereo images are
# parallel.
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
# Projection/camera matrix
# [fx' 0 cx' Tx]
# P = [ 0 fy' cy' Ty]
# [ 0 0 1 0]
# By convention, this matrix specifies the intrinsic (camera) matrix
# of the processed (rectified) image. That is, the left 3x3 portion
# is the normal camera intrinsic matrix for the rectified image.
# It projects 3D points in the camera coordinate frame to 2D pixel
# coordinates using the focal lengths (fx', fy') and principal point
# (cx', cy') - these may differ from the values in K.
# For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
# also have R = the identity and P[1:3,1:3] = K.
# For a stereo pair, the fourth column [Tx Ty 0]' is related to the
# position of the optical center of the second camera in the first
# camera's frame. We assume Tz = 0 so both cameras are in the same
# stereo image plane. The first camera always has Tx = Ty = 0. For
# the right (second) camera of a horizontal stereo pair, Ty = 0 and
# Tx = -fx' * B, where B is the baseline between the cameras.
# Given a 3D point [X Y Z]', the projection (x, y) of the point onto
# the rectified image is given by:
# [u v w]' = P * [X Y Z 1]'
# x = u / w
# y = v / w
# This holds for both images of a stereo pair.
msg.P = self._p
#[self._fx, 0.0, self._cx, 0.0, 0.0, self._fx, self._cy, 0.0, 0.0, 0.0, 1.0, 0.0]
msg.binning_x = 0
msg.binning_y = 0
#msg.roi = msg_roi
self._pub_info.publish(msg)
#return msg
def shutdown(self):
pass
def run(self):
c=0
while not rospy.is_shutdown():
self.rate.sleep()
if c%50==0:
self.refresh()
print c
if rospy.is_shutdown():
break
self.pubcaminfomsg()
c=c+1
if __name__=="__main__":
'''
# 初始化ros节点
rospy.init_node("node_zzztestsrv")
# 创建客户端对象
client1 = rospy.ServiceProxy("/zkhy_stereo/get_camera_params",CameraParams)
client2 = rospy.ServiceProxy("/zkhy_stereo/set_frame_rate",FrameRate)
client3 = rospy.ServiceProxy("/zkhy_stereo/get_rotation_matrix",RotationMatrix)
response = client1.call()
print("/zkhy_stereo/get_camera_params callok")
print(response)
cx=response.cx
cy=response.cy
response = client2.call(10.0,)
print("/zkhy_stereo/set_frame_rate callok")
print(response)
response = client3.call()
print("/zkhy_stereo/get_rotation_matrix callok")
print(response)
#print(response.real3DToImage)
'''
try:
m=ZKHYCamInfoNode()
m.run()
except rospy.ROSInterruptException:
pass
#'''
标定板配置之后的数据
/zkhy_stereo/get_camera_params callok
focus: 924.560392664
cx: 652.410320649
cy: 383.193440768
RRoll: 0.0
RPitch: 0.0
RYaw: 0.0
Tx: 120.910602422
Ty: 0.0
Tz: 0.0
/zkhy_stereo/set_frame_rate callok
/zkhy_stereo/get_rotation_matrix callok
real3DToImage: [952.0765991210938, 5.438984394073486, 611.094482421875, 46.22802734375, 16.343040466308594, 927.7232055664062, 374.91796875, 0.0,
0.0435481071472168, 0.008342000655829906, 0.999016523361206, 0.0]
imageToReal3D: [0.00108056899625808, -3.9293402664952737e-07, -0.6608323454856873, 4.758225063961063e-13, 0.0010815574787557125, -0.40589451789855957, -4.710305802291259e-05, -9.014106581162196e-06, 1.0331801176071167]
输出camerainfo
rostopic echo /node_zzztestsrv/camera_info
header:
seq: 1
stamp:
secs: 0
nsecs: 0
frame_id: "stereo_left"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [924.5603926636185, 0.0, 652.4103206490715, 0.0, 924.5603926636185, 383.1934407680866, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [952.0765991210938, 5.438984394073486, 611.094482421875, 46.22802734375, 16.343040466308594, 927.7232055664062, 374.91796875, 0.0, 0.0435481071472168, 0.008342000655829906, 0.999016523361206, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False
---
配置相关坐标系外参,效果概览
近处的障碍点云没有?
如有错误请指正。