目录
一、C++图片——图片增强
1.CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(image_enhance)
add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS roscpp cv_bridge image_transport sensor_msgs roscpp rospy std_msgs)
find_package(OpenCV REQUIRED)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES my_image_transport
# CATKIN_DEPENDS cv_bridge image_transport sensor_msgs
# DEPENDS system_lib
)
include_directories( ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} )
add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}.cpp)
add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
2.package.xml
<!-- -->
<launch>
<node pkg="image_enhance" type="image_enhance_node" name="image_enhance_node" output="screen">
<param name ="image_topic" value="/pub_t"/>
<param name ="frame_rate" value="30"/>
<param name ="mode" value="2"/>
</node>
</launch>
3.launch 文件
<!-- -->
<launch>
<node pkg="image_enhance" type="image_enhance_node" name="image_enhance_node" output="screen">
<param name ="image_topic" value="/pub_t"/>
<param name ="frame_rate" value="30"/>
<param name ="mode" value="2"/>
</node>
</launch>4.cpp
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>//用于ROS图像和OpenCV图像的转换
#include <sensor_msgs/CameraInfo.h>//传感器信息
#include<cmath>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <string>
#define _NODE_NAME_ "image_enhancement" //定义节点的名称
//OpenCV的函数都位于cv这一命名空间下，为了调用OpenCV的函数，需要在每个函数前加上cv::，向编译器说明你所调用的函数处于cv命名空间。为了摆脱这种繁琐的工作，可以使用using namespace cv;指令，告诉编译器假设所有函数都位于cv命名空间下。
using namespace cv;
using namespace std;
class ImageEnhancement //节点参数类
{
private://基本参数
ros::Publisher enhance_image_pub_;//发布者
ros::Subscriber image_sub_;//接收者
std::string image_topic_;//信息类型
bool is_show_result_;
bool image_ok_, image_enhanced_;
int frame_rate_;
int mode_;
//0 for 基于直方图均衡化的图像增强
//1 for 基于对数Log变换的图像增强
//2 for 基于伽马变换的图像增强
//3 for 混合增强
cv_bridge::CvImagePtr cv_ptr_;
cv_bridge::CvImagePtr cv_ptr2;
ros::Timer timer_;
public://基本函数
bool init();
void loadimage(const sensor_msgs::ImageConstPtr& msg);
void enhancepub0(const ros::TimerEvent&);
void enhancepub1(const ros::TimerEvent&);
void enhancepub2(const ros::TimerEvent&);
void enhancepub3(const ros::TimerEvent&);
};
int main(int argc, char** argv)
{
ros::init(argc, argv, _NODE_NAME_);
ImageEnhancement enhance;//创建一个类的对象
enhance.init();//调用类的成员函数
ros::spin();//循环
return 0;
}
bool ImageEnhancement::init()//定义ImageEnhancement类的成员函数
{
ros::NodeHandle nh, nh_private("~");//开启节点对象nh
//节点的参数服务器，写在launch文件中的可以随时修改的参数
nh_private.param<std::string>("image_topic", image_topic_, "");
nh_private.param<int>("frame_rate",frame_rate_,30);
nh_private.param<int>("mode",mode_,0);
image_ok_ = false;//一个关闭标志
image_enhanced_ = false;
enhance_image_pub_ = nh.advertise<sensor_msgs::Image>("/image_enhancement", 1);//定义发布者
image_sub_ = nh.subscribe(image_topic_, 1, &ImageEnhancement::loadimage, this);//定义订阅image_topic_话题的消息，回调函数，以指针形式存储数据 回调函数在一个类中，后面加上this
if(mode_ == 0)
timer_ = nh.createTimer(ros::Duration(0.01), &ImageEnhancement::enhancepub0, this);
else if(mode_ == 1)
timer_ = nh.createTimer(ros::Duration(0.01), &ImageEnhancement::enhancepub1, this);
else if(mode_ == 2)
timer_ = nh.createTimer(ros::Duration(0.01), &ImageEnhancement::enhancepub2, this);
else
ROS_ERROR("none mode is starting!");//报错打印指令
ROS_INFO("image_enhancement initial ok.");//提示打印指令
}
void ImageEnhancement::loadimage(const sensor_msgs::ImageConstPtr& msg)
{
ROS_INFO("[%s]: getting image!",_NODE_NAME_);
cv_bridge::CvImagePtr cv;//在CVbridge类中创建一个对象cv
cv = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);//转化储存图像
cv_ptr_ = cv;
image_ok_ = true;
image_enhanced_ = false;
}
void ImageEnhancement::enhancepub0(const ros::TimerEvent&)
{
if (image_ok_ == false)
{
ROS_ERROR("[%s]: no image input!",_NODE_NAME_);
return;
}
else if (image_enhanced_ == true)
{
ROS_INFO("[%s]: waiting for new image!",_NODE_NAME_);
return;
}
else
ROS_INFO("[%s]: image enhancement start! mode:0",_NODE_NAME_);
static int width, height;
width = cv_ptr_->image.cols;
height = cv_ptr_->image.rows;
//创建一个enhanced_image对象
cv::Mat enhanced_image(height, width, CV_8UC3);//cv::Mat是OpenCV2和OpenCV3中基本的数据类型长宽和文件格式
enhanced_image.setTo(0);//初始化增强后的图
cv_ptr_->image.copyTo(enhanced_image(Rect(0, 0, width, height)));
cv::Mat imageRGB[3];
split(enhanced_image, imageRGB);
for (int i=0; i<3; ++i)
{
equalizeHist(imageRGB[i], imageRGB[i]);
}
merge(imageRGB, 3, enhanced_image);
//定义发布消息的内容
sensor_msgs::ImagePtr imageMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", enhanced_image).toImageMsg();
imageMsg->header.frame_id = std::string("enhance image");
imageMsg->header.stamp = ros::Time::now();
enhance_image_pub_.publish(imageMsg);//发布消息
ROS_INFO("[%s]: image enhancement done!",_NODE_NAME_);
image_enhanced_ = true;
}
void ImageEnhancement::enhancepub1(const ros::TimerEvent&)
{
if (image_ok_ == false)
{
ROS_ERROR("[%s]: no image input!",_NODE_NAME_);
return;
}
else if (image_enhanced_ == true)
{
ROS_INFO("[%s]: waiting for new image!",_NODE_NAME_);
return;
}
else
ROS_INFO("[%s]: image enhancement start! mode:0",_NODE_NAME_);
static int width, height;
width = cv_ptr_->image.cols;
height = cv_ptr_->image.rows;
cv::Mat enhanced_image(height, width, CV_8UC3);
enhanced_image.setTo(0);
cv_ptr_->image.copyTo(enhanced_image(Rect(0, 0, width, height)));
cv::Mat srcLog(enhanced_image.size(), CV_32FC3);
for (int i=0; i<enhanced_image.rows; ++i)
{
for (int j=0; j<enhanced_image.cols; ++j)
{
srcLog.at<Vec3f>(i, j)[0] = log(1 + enhanced_image.at<Vec3b>(i, j)[0]);
srcLog.at<Vec3f>(i, j)[1] = log(1 + enhanced_image.at<Vec3b>(i, j)[1]);
srcLog.at<Vec3f>(i, j)[2] = log(1 + enhanced_image.at<Vec3b>(i, j)[2]);
}
}
//归一化
normalize(srcLog, srcLog, 0, 255, NORM_MINMAX);
//
convertScaleAbs(srcLog, srcLog);
sensor_msgs::ImagePtr imageMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", srcLog).toImageMsg();
imageMsg->header.frame_id = std::string("enhance image");
imageMsg->header.stamp = ros::Time::now();
enhance_image_pub_.publish(imageMsg);
ROS_INFO("[%s]: image enhancement done!",_NODE_NAME_);
image_enhanced_ = true;
}
void ImageEnhancement::enhancepub2(const ros::TimerEvent&)
{
if (image_ok_ == false)
{
ROS_ERROR("[%s]: no image input!",_NODE_NAME_);
return;
}
else if (image_enhanced_ == true)
{
ROS_INFO("[%s]: waiting for new image!",_NODE_NAME_);
return;
}
else
ROS_INFO("[%s]: image enhancement start! mode:0",_NODE_NAME_);
static int width, height;
width = cv_ptr_->image.cols;
height = cv_ptr_->image.rows;
cv::Mat enhanced_image(height, width, CV_8UC3);
enhanced_image.setTo(0);
cv_ptr_->image.copyTo(enhanced_image(Rect(0, 0, width, height)));
float pixels[256];
for (int i=0; i<256; ++i)
{
pixels[i] = powf(i,2);
}
cv::Mat srcLog(enhanced_image.size(), CV_32FC3);
for (int i=0; i<enhanced_image.rows; ++i)
{
for (int j=0; j<enhanced_image.cols; ++j)
{
srcLog.at<Vec3f>(i, j)[0] = pixels[enhanced_image.at<Vec3b>(i, j)[0]];
srcLog.at<Vec3f>(i, j)[1] = pixels[enhanced_image.at<Vec3b>(i, j)[1]];
srcLog.at<Vec3f>(i, j)[2] = pixels[enhanced_image.at<Vec3b>(i, j)[2]];
}
}
normalize(srcLog, srcLog, 0, 255, NORM_MINMAX);
//Mat img;
//resize(srcLog, img, Size(640, 360), 0, 0, INTER_CUBIC);
convertScaleAbs(srcLog, srcLog, 4, 30);
resize(srcLog, srcLog,Size(1920,1440),INTER_LINEAR);
medianBlur(srcLog,srcLog, 3);
sensor_msgs::ImagePtr imageMsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8",srcLog).toImageMsg();
imageMsg->header.frame_id = std::string("enhance image");
imageMsg->header.stamp = ros::Time::now();
enhance_image_pub_.publish(imageMsg);
image_enhanced_ = true;
ROS_INFO("[%s]: image enhancement done!",_NODE_NAME_);
}二、C++点云——点云投影
1.CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(kittivelo_cam)
add_compile_options(-std=c++11)
set(CMAKE_BUILD_TYPE Release)
find_package(catkin REQUIRED COMPONENTS
cv_bridge
image_transport
message_filters
roscpp
rospy
std_msgs
tf
pcl_conversions
tf2
tf2_ros
tf2_geometry_msgs
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES colored_pointcloud
# CATKIN_DEPENDS cv_bridge image_transport message_filters roscpp rospy std_msgs
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
# opencv
find_package(OpenCV 3 REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# pcl
find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})
# boost
find_package(Boost REQUIRED)
include_directories(${Boost_INCLUDE_DIRS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
add_executable(kittivelo_cam src/kittivelo_cam.cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(kittivelo_cam ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_LIBRARIES})2.package.xml
<?xml version="1.0"?>
<package format="2">
<name>kittivelo_cam</name>
<version>0.0.0</version>
<description>The colored_pointcloud package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="imrs@todo.todo">imrs</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/colored_pointcloud</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>message_filters</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>cv_bridge</build_export_depend>
<build_export_depend>image_transport</build_export_depend>
<build_export_depend>message_filters</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>image_transport</exec_depend>
<exec_depend>message_filters</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>3. launch
<!-- -->
<launch>
<node pkg="kittivelo_cam" type="kittivelo_cam" name="kittivelo_cam" output="screen">
</node>
</launch>4. cpp
//
// Created by cai on 2021/8/26.
//
#include<ros/ros.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<image_transport/image_transport.h>
#include <time.h>
#include<opencv2/opencv.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<cmath>
#include<stdio.h>
//#include "fssim_common/Cmd.h"
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
#include <ros/ros.h>
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <boost/thread/thread.hpp>
#include <iostream>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/common/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <tf2_ros/transform_listener.h>
#include <sensor_msgs/point_cloud_conversion.h>
using namespace Eigen;
using namespace cv;
using namespace std;
#include "colored_pointcloud/colored_pointcloud.h"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
cv::Mat P_rect_00(3,4,cv::DataType<double>::type);//3×4 projection matrix after rectification
cv::Mat R_rect_00(4,4,cv::DataType<double>::type);//3×3 rectifying rotation to make image planes co-planar
cv::Mat RT(4,4,cv::DataType<double>::type);//rotation matrix and translation vector
class RsCamFusion
{
//**********************************************************************************************************
//1、定义成员变量
private:
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,sensor_msgs::PointCloud2> slamSyncPolicy;
message_filters::Synchronizer<slamSyncPolicy>* sync_;
message_filters::Subscriber<sensor_msgs::Image>* camera_sub1;
message_filters::Subscriber<sensor_msgs::PointCloud2>* lidar_sub;
pcl::PointCloud<PointXYZRGBI>::Ptr colored_cloud_toshow;
pcl::PointCloud<PointXYZRGBI>::Ptr colored_cloud;
pcl::PointCloud<PointXYZRGBI>::Ptr cloud_toshow;
/*
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud_toshow;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_toshow;
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud;
pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud_ptr;
pcl::PointCloud<pcl::PointXYZI>::Ptr raw_cloud;
cv::Mat input_image;
cv::Mat image_to_show,image_to_show1;
int frame_count = 0;
static cv::Size imageSize;
static ros::Publisher pub;
//store calibration data in Opencv matrices
image_transport::Publisher depth_pub ;
sensor_msgs::ImagePtr depth_msg;
ros::NodeHandle nh;
ros::Publisher colored_cloud_showpub;
ros::Subscriber sub;
ros::Publisher fused_image_pub1;
public:
//构造函数
RsCamFusion():
nh("~"){
RT.at<double>(0,0) = 7.533745e-03;RT.at<double>(0,1) = -9.999714e-01;RT.at<double>(0,2) = -6.166020e-04;RT.at<double>(0,2) = -4.069766e-03;
RT.at<double>(1,0) = 1.480249e-02;RT.at<double>(1,1) = 7.280733e-04;RT.at<double>(1,2) = -9.998902e-01;RT.at<double>(1,3) = -7.631618e-02;
RT.at<double>(2,0) = 9.998621e-01;RT.at<double>(2,1) = 7.523790e-03;RT.at<double>(2,2) = 1.480755e-02;RT.at<double>(2,3) = -2.717806e-01;
RT.at<double>(3,0) = 0.0;RT.at<double>(3,1) = 0.0;RT.at<double>(3,2) = 0.0;RT.at<double>(3,3) = 1.0;
R_rect_00.at<double>(0,0) = 9.999239e-01;R_rect_00.at<double>(0,1) = 9.837760e-03;R_rect_00.at<double>(0,2) = -7.445048e-03;R_rect_00.at<double>(0,3) = 0.0;
R_rect_00.at<double>(1,0) = -9.869795e-03;R_rect_00.at<double>(1,1) = 9.999421e-01;R_rect_00.at<double>(1,2) = -4.278459e-03;R_rect_00.at<double>(1,3) = 0.0;
R_rect_00.at<double>(2,0) = 7.402527e-03;R_rect_00.at<double>(2,1) = 4.351614e-03;R_rect_00.at<double>(2,2) = 9.999631e-01;R_rect_00.at<double>(2,3) = 0.0;
R_rect_00.at<double>(3,0) = 0.0;R_rect_00.at<double>(3,1) = 0.0;R_rect_00.at<double>(3,2) = 0.0;R_rect_00.at<double>(3,3) = 1.0;
P_rect_00.at<double>(0,0) = 7.215377e+02;P_rect_00.at<double>(0,1) = 0.000000e+00;P_rect_00.at<double>(0,2) = 6.095593e+02;P_rect_00.at<double>(0,3) = 0.000000e+00;
P_rect_00.at<double>(1,0) = 0.000000e+00;P_rect_00.at<double>(1,1) = 7.215377e+02;P_rect_00.at<double>(1,2) = 1.728540e+02;P_rect_00.at<double>(1,3) = 0.000000e+00;
P_rect_00.at<double>(2,0) = 0.000000e+00;P_rect_00.at<double>(2,1) = 0.000000e+00;P_rect_00.at<double>(2,2) = 1.000000e+00;P_rect_00.at<double>(2,3) = 0.000000e+00;
camera_sub1 = new message_filters::Subscriber<sensor_msgs::Image>(nh, "/forward",300);
lidar_sub = new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/kitti/velo/pointcloud",100);
sync_ = new message_filters::Synchronizer<slamSyncPolicy>(slamSyncPolicy(100), *camera_sub1,*lidar_sub);
sync_->registerCallback(boost::bind(&RsCamFusion::callback,this, _1, _2));
cout<<"waite_image"<<endl;
allocateMemory(); //初始化
}
void allocateMemory()
{
raw_cloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
colored_cloud_toshow.reset(new pcl::PointCloud<PointXYZRGBI>());
colored_cloud.reset(new pcl::PointCloud<PointXYZRGBI>());
cloud_toshow.reset(new pcl::PointCloud<PointXYZRGBI>());
input_cloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
input_cloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZI>());
}
void resetParameters(){
raw_cloud->clear();
input_cloud_ptr->clear();
input_cloud->clear();
colored_cloud_toshow->clear();
colored_cloud->clear();
cloud_toshow->clear();
}
void callback(const sensor_msgs::ImageConstPtr input_image_msg1,
const sensor_msgs::PointCloud2ConstPtr input_cloud_msg)
{
resetParameters();
cv::Mat input_image1;
cv_bridge::CvImagePtr cv_ptr1;
std_msgs::Header image_header1 = input_image_msg1->header;
std_msgs::Header cloud_header = input_cloud_msg->header;
//数据获取
//图像ROS消息转化
cv_ptr1 = cv_bridge::toCvCopy(input_image_msg1,sensor_msgs::image_encodings::BGR8);
input_image1 = cv_ptr1->image;
//获取点云
pcl::fromROSMsg(*input_cloud_msg, *input_cloud_ptr);//把input_cloud_msg放
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*input_cloud_ptr, *input_cloud_ptr, indices);//去除无效点
transformpoint(input_cloud_ptr,input_image1,P_rect_00,R_rect_00,RT);
cout<<"start"<<endl;
colored_cloud_showpub = nh.advertise<sensor_msgs::PointCloud2>("colored_cloud_toshow",10);
publishCloudtoShow(colored_cloud_showpub, cloud_header, colored_cloud_toshow);
fused_image_pub1 = nh.advertise<sensor_msgs::Image>("image_to_show",10);
publishImage(fused_image_pub1, image_header1, image_to_show1);
frame_count = frame_count + 1;
}
//××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××
void transformpoint(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& input_cloud, const cv::Mat input_image, cv::Mat &P_rect_00,cv::Mat &R_rect_00,cv::Mat &RT)
{
cv::Mat X(4,1,cv::DataType<double>::type);
cv::Mat Y(4,1,cv::DataType<double>::type);
cv::Point pt;
std::vector<cv::Point3f> rawPoints;
*raw_cloud = *input_cloud;
image_to_show = input_image.clone();
for(int i=0;i<raw_cloud->size();i++) {
// convert each 3D point into homogeneous coordinates and store it in a 4D variable X
X.at<double>(0, 0) = raw_cloud->points[i].x;
X.at<double>(1, 0) = raw_cloud->points[i].y;
X.at<double>(2, 0) = raw_cloud->points[i].z;
X.at<double>(3, 0) = 1;
//apply the projection equation to map X onto the image plane of the camera. Store the result in Y
//计算矩阵
Y=P_rect_00*R_rect_00*RT*X;
pt.x=Y.at<double>(0, 0) / Y.at<double>(2, 0);
pt.y=Y.at<double>(1, 0) / Y.at<double>(2, 0);
// transform Y back into Euclidean coordinates and store the result in the variable pt
float d = Y.at<double>(2, 0)*1000.0;
float val = raw_cloud->points[i].x;
float maxVal = 20.0;
int red = min(255, (int) (255 * abs((val - maxVal) / maxVal)));
int green = min(255, (int) (255 * (1 - abs((val - maxVal) / maxVal))));
if(pt.x<1240 &&pt.x>0 &&pt.y<375 &&pt.y>0 &&d>0)
{
/*
if (int(input_image.at<cv::Vec3b>(pt.y, pt.x)[2]) == 128 || int(input_image.at<cv::Vec3b>(pt.y, pt.x)[2]) == 152
||int(input_image.at<cv::Vec3b>(pt.y, pt.x)[2]) == 107 || int(input_image.at<cv::Vec3b>(pt.y, pt.x)[2]) == 244
||int(input_image.at<cv::Vec3b>(pt.y, pt.x)[2]) == 70 )
{
*/
cv::circle(image_to_show, pt, 1, cv::Scalar(0, green, red), cv::FILLED);
image_to_show1 = image_to_show;
PointXYZRGBI point;
point.x = raw_cloud->points[i].x;
point.y = raw_cloud->points[i].y; //to create colored point clouds
point.z = raw_cloud->points[i].z;
point.intensity = raw_cloud->points[i].intensity;
point.g = input_image.at<cv::Vec3b>(pt.y, pt.x)[1];
point.b = input_image.at<cv::Vec3b>(pt.y, pt.x)[0];
point.r = input_image.at<cv::Vec3b>(pt.y, pt.x)[2];
colored_cloud->points.push_back(point);
}
/*
else
{
pcl::PointXYZRGB point;
point.x = raw_cloud->points[i].x;
point.y = raw_cloud->points[i].y; //to create colored point clouds
point.z = raw_cloud->points[i].z;
//point.intensity = raw_cloud->points[i].intensity;
point.g = 0;
point.b = 0;
point.r = 0;
cloud_toshow->points.push_back(point);
}
*/
}
*colored_cloud_toshow=*colored_cloud+*cloud_toshow;
}
void publishCloudtoShow(const ros::Publisher& cloudtoshow_pub, const std_msgs::Header& header,
const pcl::PointCloud<PointXYZRGBI>::ConstPtr& cloud)
{
sensor_msgs::PointCloud2 output_msg;
pcl::toROSMsg(*cloud, output_msg);
output_msg.header = header;
cloudtoshow_pub.publish(output_msg);
}
void publishImage(const ros::Publisher& image_pub, const std_msgs::Header& header, const cv::Mat image)
{
cv_bridge::CvImage output_image;
output_image.header = header;
output_image.encoding = sensor_msgs::image_encodings::TYPE_8UC3;
output_image.image = image;
image_pub.publish(output_image);
}
};
//*****************************************************************************************************
//
// 程序入口
//
//×××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××
int main(int argc, char** argv)
{
//1、节点初始化 及定义参数
ros::init(argc, argv, "kitti3D2");
RsCamFusion RF;
ros::spin();
return 0;
}三、python——yolov5神经网络
#!/usr/bin/env python
# YOLOv5 🚀 by Ultralytics, GPL-3.0 license
"""
Run inference on images, videos, directories, streams, etc.
Usage - sources:
$ python path/to/detect.py --weights yolov5s.pt --source 0 # webcam
img.jpg # image
vid.mp4 # video
path/ # directory
path/*.jpg # glob
'https://youtu.be/Zgi9g1ksQHc' # YouTube
'rtsp://example.com/media.mp4' # RTSP, RTMP, HTTP stream
Usage - formats:
$ python path/to/detect.py --weights yolov5s.pt # PyTorch
yolov5s.torchscript # TorchScript
yolov5s.onnx # ONNX Runtime or OpenCV DNN with --dnn
yolov5s.xml # OpenVINO
yolov5s.engine # TensorRT
yolov5s.mlmodel # CoreML (MacOS-only)
yolov5s_saved_model # TensorFlow SavedModel
yolov5s.pb # TensorFlow GraphDef
yolov5s.tflite # TensorFlow Lite
yolov5s_edgetpu.tflite # TensorFlow Edge TPU
"""
import os
import roslib
import rospy
from std_msgs.msg import Header
from std_msgs.msg import String
from sensor_msgs.msg import Image
import numpy as np
import argparse
import os
import sys
from pathlib import Path
import cv2
import torch
import torch.backends.cudnn as cudnn
FILE = Path(__file__).resolve()
ROOT = FILE.parents[0] # YOLOv5 root directory
if str(ROOT) not in sys.path:
sys.path.append(str(ROOT)) # add ROOT to PATH
ROOT = Path(os.path.relpath(ROOT, Path.cwd())) # relative
from models.common import DetectMultiBackend
from utils.datasets import IMG_FORMATS, VID_FORMATS, LoadImages, LoadStreams
from utils.general import (LOGGER, check_file, check_img_size, check_imshow, check_requirements, colorstr,
increment_path, non_max_suppression, print_args, scale_coords, strip_optimizer, xyxy2xywh)
from utils.plots import Annotator, colors, save_one_box
from utils.torch_utils import select_device, time_sync
@torch.no_grad()
class SubscribeAndPublish:
def __init__(self):
self.all_obstacle_str=''
self.sub1_name="/cam_rgb1/usb_cam/image_raw"
self.sub1= rospy.Subscriber(self.sub1_name, Image,self.callback_rgb)
self.pub1_name="detect_rgb"
self.pub1= rospy.Publisher(self.pub1_name, Image,queue_size=1)
self.model=model
self.device=device
self.img_rgb=[]
def callback_rgb(self,data):
print('callback1')
img_rgb = np.frombuffer(data.data, dtype=np.uint8).reshape(data.height, data.width, -1)
img_rgb=img_rgb[:,:,::-1]
self.img_rgb=img_rgb
cv2.imwrite('./temp/rgb/rgb.jpg',img_rgb)
img_rgb=self.run(**vars(opt))
if len(img_rgb)>0:
print('send img')
self.publish_image(self.pub1,img_rgb,'base_link')
def publish_image(self,pub, data, frame_id='base_link'):
assert len(data.shape) == 3, 'len(data.shape) must be equal to 3.'
header = Header(stamp=rospy.Time.now())
header.frame_id = frame_id
msg = Image()
msg.height = data.shape[0]
msg.width = data.shape[1]
msg.encoding = 'rgb8'
msg.data = np.array(data).tostring()
msg.header = header
msg.step = msg.width * 1 * 3
pub.publish(msg)
def run(self, weights=ROOT / 'yolov5s.pt', # model.pt path(s) #在类里面+self
source=ROOT / './temp/rgb/', # file/dir/URL/glob, 0 for webcam
data=ROOT / 'data/coco128.yaml', # dataset.yaml path
imgsz=(640, 640), # inference size (height, width)
conf_thres=0.25, # confidence threshold
iou_thres=0.45, # NMS IOU threshold
max_det=1000, # maximum detections per image
device='', # cuda device, i.e. 0 or 0,1,2,3 or cpu
view_img=False, # show results
save_txt=False, # save results to *.txt
save_conf=False, # save confidences in --save-txt labels
save_crop=False, # save cropped prediction boxes
nosave=False, # do not save images/videos
classes=None, # filter by class: --class 0, or --class 0 2 3
agnostic_nms=False, # class-agnostic NMS
augment=False, # augmented inference
visualize=False, # visualize features
update=False, # update all models
project=ROOT / 'runs/detect', # save results to project/name
name='exp', # save results to project/name
exist_ok=False, # existing project/name ok, do not increment
line_thickness=3, # bounding box thickness (pixels)
hide_labels=False, # hide labels
hide_conf=False, # hide confidences
half=False, # use FP16 half-precision inference
dnn=False, # use OpenCV DNN for ONNX inference
):
source = str(source)
save_img = not nosave and not source.endswith('.txt') # save inference images
is_file = Path(source).suffix[1:] in (IMG_FORMATS + VID_FORMATS)
is_url = source.lower().startswith(('rtsp://', 'rtmp://', 'http://', 'https://'))
webcam = source.isnumeric() or source.endswith('.txt') or (is_url and not is_file)
if is_url and is_file:
source = check_file(source) # download
# Directories
save_dir = increment_path(Path(project) / name, exist_ok=exist_ok) # increment run
# (save_dir / 'labels' if save_txt else save_dir).mkdir(parents=True, exist_ok=True) # make dir
# Load model
stride, names, pt = model.stride, model.names, model.pt
imgsz = check_img_size(imgsz, s=stride) # check image size
# Dataloader
if webcam:
view_img = check_imshow()
cudnn.benchmark = True # set True to speed up constant image size inference
dataset = LoadStreams(source, img_size=imgsz, stride=stride, auto=pt)
bs = len(dataset) # batch_size
else:
dataset = LoadImages(source, img_size=imgsz, stride=stride, auto=pt)
bs = 1 # batch_size
vid_path, vid_writer = [None] * bs, [None] * bs
# Run inference
model.warmup(imgsz=(1 if pt else bs, 3, *imgsz)) # warmup
dt, seen = [0.0, 0.0, 0.0], 0
for path, im, im0s, vid_cap, s in dataset:
t1 = time_sync()
im=torch.from_numpy(im).to(self.device)
im = im.half() if model.fp16 else im.float() # uint8 to fp16/32
im /= 255 # 0 - 255 to 0.0 - 1.0
if len(im.shape) == 3:
im = im[None] # expand for batch dim
t2 = time_sync()
dt[0] += t2 - t1
# Inference
visualize = increment_path(save_dir / Path(path).stem, mkdir=True) if visualize else False
pred = model(im, augment=augment, visualize=visualize)
t3 = time_sync()
dt[1] += t3 - t2
# NMS
pred = non_max_suppression(pred, conf_thres, iou_thres, classes, agnostic_nms, max_det=max_det)
dt[2] += time_sync() - t3
# Second-stage classifier (optional)
# pred = utils.general.apply_classifier(pred, classifier_model, im, im0s)
# Process predictions
for i, det in enumerate(pred): # per image
seen += 1
if webcam: # batch_size >= 1
p, im0, frame = path[i], im0s[i].copy(), dataset.count
s += f'{i}: '
else:
p, im0, frame = path, im0s.copy(), getattr(dataset, 'frame', 0)
p = Path(p) # to Path
save_path = str(save_dir / p.name) # im.jpg
txt_path = str(save_dir / 'labels' / p.stem) + ('' if dataset.mode == 'image' else f'_{frame}') # im.txt
s += '%gx%g ' % im.shape[2:] # print string
gn = torch.tensor(im0.shape)[[1, 0, 1, 0]] # normalization gain whwh
imc = im0.copy() if save_crop else im0 # for save_crop
annotator = Annotator(im0, line_width=line_thickness, example=str(names))
if len(det):
# Rescale boxes from img_size to im0 size
det[:, :4] = scale_coords(im.shape[2:], det[:, :4], im0.shape).round()
# Print results
for c in det[:, -1].unique():
n = (det[:, -1] == c).sum() # detections per class
s += f"{n} {names[int(c)]}{'s' * (n > 1)}, " # add to string
# Write results
for *xyxy, conf, cls in reversed(det):
if save_txt: # Write to file
xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist() # normalized xywh
line = (cls, *xywh, conf) if save_conf else (cls, *xywh) # label format
with open(txt_path + '.txt', 'a') as f:
f.write(('%g ' * len(line)).rstrip() % line + '\n')
if save_img or save_crop or view_img: # Add bbox to image
c = int(cls) # integer class
label = None if hide_labels else (names[c] if hide_conf else f'{names[c]} {conf:.2f}')
annotator.box_label(xyxy, label, color=colors(c, True))
if save_crop:
save_one_box(xyxy, imc, file=save_dir / 'crops' / names[c] / f'{p.stem}.jpg', BGR=True)
# Stream results
im0 = annotator.result()
if view_img:
cv2.imshow(str(p), im0)
cv2.waitKey(1) # 1 millisecond
# Save results (image with detections)
if save_img:
if dataset.mode == 'image':
cv2.imwrite(save_path, im0)
else: # 'video' or 'stream'
if vid_path[i] != save_path: # new video
vid_path[i] = save_path
if isinstance(vid_writer[i], cv2.VideoWriter):
vid_writer[i].release() # release previous video writer
if vid_cap: # video
fps = vid_cap.get(cv2.CAP_PROP_FPS)
w = int(vid_cap.get(cv2.CAP_PROP_FRAME_WIDTH))
h = int(vid_cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
else: # stream
fps, w, h = 30, im0.shape[1], im0.shape[0]
save_path = str(Path(save_path).with_suffix('.mp4')) # force *.mp4 suffix on results videos
vid_writer[i] = cv2.VideoWriter(save_path, cv2.VideoWriter_fourcc(*'mp4v'), fps, (w, h))
vid_writer[i].write(im0)
# Print time (inference-only)
LOGGER.info(f'{s}Done. ({t3 - t2:.3f}s)')
# Print results
t = tuple(x / seen * 1E3 for x in dt) # speeds per image
LOGGER.info(f'Speed: %.1fms pre-process, %.1fms inference, %.1fms NMS per image at shape {(1, 3, *imgsz)}' % t)
if save_txt or save_img:
s = f"\n{len(list(save_dir.glob('labels/*.txt')))} labels saved to {save_dir / 'labels'}" if save_txt else ''
LOGGER.info(f"Results saved to {colorstr('bold', save_dir)}{s}")
if update:
strip_optimizer(weights) # update model (to fix SourceChangeWarning)
return im0
def parse_opt():
parser = argparse.ArgumentParser()
parser.add_argument('--weights', type=str, default=ROOT / '/home/cxl/ros_yolov5/src/yolov5/weights/yolov5x.pt', help='model path(s)')
parser.add_argument('--source', type=str, default=ROOT / './temp/rgb/', help='file/dir/URL/glob, 0 for webcam')
parser.add_argument('--data', type=str, default=ROOT / 'data/coco128.yaml', help='(optional) dataset.yaml path')
parser.add_argument('--imgsz', '--img', '--img-size', nargs='+', type=int, default=[640], help='inference size h,w')
parser.add_argument('--conf-thres', type=float, default=0.25, help='confidence threshold')#概率大于0.25显示出来
parser.add_argument('--iou-thres', type=float, default=0.45, help='NMS IoU threshold')#检测框的概率
parser.add_argument('--max-det', type=int, default=1000, help='maximum detections per image')
parser.add_argument('--device', default='', help='cuda device, i.e. 0 or 0,1,2,3 or cpu')
parser.add_argument('--view-img', action='store_true', help='show results')#实时查看结果
parser.add_argument('--save-txt', action='store_true', help='save results to *.txt')#保存标注结果
parser.add_argument('--save-conf', action='store_true', help='save confidences in --save-txt labels')#保存标注结果
parser.add_argument('--save-crop', action='store_true', help='save cropped prediction boxes') #保存置信度
parser.add_argument('--nosave', action='store_true', help='do not save images/videos')
parser.add_argument('--classes', nargs='+', type=int, help='filter by class: --classes 0, or --classes 0 2 3') #只检测特定类别
parser.add_argument('--agnostic-nms', action='store_true', help='class-agnostic NMS')
parser.add_argument('--augment', action='store_true', help='augmented inference')#增强算法
parser.add_argument('--visualize', action='store_true', help='visualize features')
parser.add_argument('--update', action='store_true', help='update all models')
parser.add_argument('--project', default=ROOT / 'runs/detect', help='save results to project/name')#结果保存在什么位置
parser.add_argument('--name', default='exp', help='save results to project/name')
parser.add_argument('--exist-ok', action='store_true', help='existing project/name ok, do not increment')#保存在原文件夹还是新文件夹
parser.add_argument('--line-thickness', default=3, type=int, help='bounding box thickness (pixels)')
parser.add_argument('--hide-labels', default=False, action='store_true', help='hide labels')
parser.add_argument('--hide-conf', default=False, action='store_true', help='hide confidences')
parser.add_argument('--half', action='store_true', help='use FP16 half-precision inference')
parser.add_argument('--dnn', action='store_true', help='use OpenCV DNN for ONNX inference')
opt = parser.parse_args() #参数都会放到opt
opt.imgsz *= 2 if len(opt.imgsz) == 1 else 1 # expand
print_args(FILE.stem, opt)
return opt
def main(opt,model,device):
rospy.init_node('yolov5', anonymous=True)
#####################
t=SubscribeAndPublish()
#####################
rospy.spin()
if __name__ == "__main__":
opt = parse_opt()
device = ''
weights = '/home/cxl/ros_yolov5/src/yolov5/weights/yolov5x.pt'
dnn=False
device = select_device(device)
model = DetectMultiBackend(weights, device=device, dnn=dnn)
main(opt,model,device)四、python——图片除雾
#!/usr/bin/env python
import os
import roslib
import rospy
from std_msgs.msg import Header
from std_msgs.msg import String
from sensor_msgs.msg import Image
import cv2
import numpy as np
import argparse
class SubscribeAndPublish:
def __init__(self):
print("waiting image")
self.all_obstacle_str=''
self.sub1_name="/image_get"
self.sub1= rospy.Subscriber(self.sub1_name, Image,self.callback)
self.pub1_name="refog_image"
self.pub1= rospy.Publisher(self.pub1_name, Image,queue_size=1)
self.img = []
def callback(self,data):
print('callback')
global i
img = np.frombuffer(data.data, dtype=np.uint8).reshape(data.height, data.width, -1)
img = img[:,:,::-1]
img = cv2.resize(img, (720, 405), interpolation=cv2.INTER_LINEAR)
#img = cv2.medianBlur(img, 5)
#kernel = np.array([[0, -1, 0], [-1, 5, -1], [0, -1, 0]], np.float32) #定义一个核
#img = cv2.filter2D(img, -1, kernel=kernel)
#print(img.shape)
m = self.deHaze(img/255)*255
#print(dtype) # 处理完要float64 还有负值
m = np.clip(m,a_min=0,a_max=255) #先归一化到0-2500消除负数
#print(m)
cv2.imwrite("/home/cxl/ros_yolov5/src/datasets/result/2.png",m)
m = m.astype(np.int8) #在转化为8位数
i+=1
#print(m.dtype)
self.publish_image(self.pub1,m,'base_link')
#print('finish remove fog')
def publish_image(self,pub, data, frame_id='base_link'):
assert len(data.shape) == 3, 'len(data.shape) must be equal to 3.'
print(data.shape)
header = Header(stamp=rospy.Time.now())
header.frame_id = frame_id
msg = Image()
msg.height = data.shape[0]
msg.width = data.shape[1]
msg.encoding = 'rgb8' #传入数据要求是8位
msg.data = np.array(data).tostring()
msg.header = header
msg.step = msg.width *1*3
#print("**************************")
pub.publish(msg)
#print("---------------------------")
def zmMinFilterGray(self,src, r=7):
'''最小值滤波，r是滤波器半径'''
'''if r <= 0:
return src
h, w = src.shape[:2]
I = src
res = np.minimum(I , I[[0]+range(h-1) , :])
res = np.minimum(res, I[range(1,h)+[h-1], :])
I = res
res = np.minimum(I , I[:, [0]+range(w-1)])
res = np.minimum(res, I[:, range(1,w)+[w-1]])
return zmMinFilterGray(res, r-1)'''
return cv2.erode(src, np.ones((2*r+1, 2*r+1))) #使用opencv的erode函数更高效
def guidedfilter(self,I, p, r, eps):
'''引导滤波，直接参考网上的matlab代码'''
height, width = I.shape
m_I = cv2.boxFilter(I, -1, (r,r))
m_p = cv2.boxFilter(p, -1, (r,r))
m_Ip = cv2.boxFilter(I*p, -1, (r,r))
cov_Ip = m_Ip-m_I*m_p
m_II = cv2.boxFilter(I*I, -1, (r,r))
var_I = m_II-m_I*m_I
a = cov_Ip/(var_I+eps)
b = m_p-a*m_I
m_a = cv2.boxFilter(a, -1, (r,r))
m_b = cv2.boxFilter(b, -1, (r,r))
return m_a*I+m_b
def getV1(self,m, r, eps, w, maxV1): #输入rgb图像，值范围[0,1]
'''计算大气遮罩图像V1和光照值A, V1 = 1-t/A'''
V1 = np.min(m,2) #得到暗通道图像
V1 = self.guidedfilter(V1, self.zmMinFilterGray(V1,7), r, eps) #使用引导滤波优化
bins = 2000 #2000
ht = np.histogram(V1, bins) #计算大气光照A
d = np.cumsum(ht[0])/float(V1.size)
for lmax in range(bins-1, 0, -1):
if d[lmax]<=0.999:
break
A = np.mean(m,2)[V1>=ht[1][lmax]].max()
V1 = np.minimum(V1*w, maxV1) #对值范围进行限制
return V1,A
def imgBrightness(self,img1, a, b):
h, w, ch = img1.shape#获取shape的数值，height和width、通道
#新建全零图片数组src2,将height和width，类型设置为原图片的通道类型(色素全为零，输出为全黑图片)
src2 = np.zeros([h, w, ch], img1.dtype)
dst = cv2.addWeighted(img1, a, src2, 1-a, b)#addWeighted函数说明如下
return dst
def deHaze(self,img, r=81, eps=0.001, w=0.95, maxV1=0.80, bGamma=False): #r=81, eps=0.001, w=0.95, maxV1=0.80, bGamma=False
Y = np.zeros(img.shape)
V1,A = self.getV1(img, r, eps, w, maxV1) #得到遮罩图像和大气光照
for k in range(3):
Y[:,:,k] = (img[:,:,k]-V1)/(1-V1/A) #颜色校正
Y = np.clip(Y, 0, 1)
#if bGamma:
#Y = Y**(np.log(0.3)/np.log(Y.mean())) #gamma校正, 越大越亮 0.3
#Y = self.imgBrightness(Y, 2.8, -0.15) #第一个值是亮度，第二个是对比度 2.8 -0.15
return Y
def init():
rospy.init_node('remove_fog',anonymous=True)
t = SubscribeAndPublish()
rospy.spin()
if __name__ == '__main__':
i=1
init()
五、python——接收点云
#! /usr/bin/env python
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
import rospy
import time
def callback_pointcloud(data):
assert isinstance(data, PointCloud2)
gen = point_cloud2.read_points(data, field_names=("x", "y", "z"), skip_nans=True)
time.sleep(1)
print type(gen)
for p in gen:
print " x : %.3f y: %.3f z: %.3f" %(p[0],p[1],p[2])
def main():
rospy.init_node('pcl_listener', anonymous=True)
rospy.Subscriber('/my_camera/depth/points', PointCloud2, callback_pointcloud)
rospy.spin()
if __name__ == "__main__":
main()
500
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
