Intel RealSense SR300
 |
 |
SR300工作原理
SR300设备的红外线发射器（IR Laser Projector）发射的“结构光”，经物体反射后会被红外线传感器（IR Camera Lens）接收。由于红外线到反射物体表面的距离不同，红外传感器捕到的“结构光”图案的位置和形状会发生变化，根据这些由实感图像处理芯片就能计算出物体表面的空间信息，再用三角测距原理进行“深度”计算，进而重现3D场景
测量范围：0.2m ~ 1.2m （要求：室内，并且）
 典型“结构光”图案 |
启动rs_camera节点，读取深度相机sr300的图像
roscore
roslaunch realsense2_camera rs_camera.launch
查看/camera/depth/image_rect_raw话题和sensor_msgs/Image消息类型
rostopic info /camera/depth/image_rect_raw
配置完cv_bridge不知为何不能发布话题消息了，检查了程序也没改什么，就是加入了opencv的头文件这些。还重装了一次ros，也没用。运行了一个topic_demo,两个节点talker和listener,一个发布话题消息，一个订阅，正常运行，那就说明ros环境没问题。回去检查qt,回退到几天前的版本也不行，应该是qt出问题了。
talker发布消息，ur5_monitor能收到，listener也能收到，但qt `创建的包test2发布消息，listener就收不到，但订阅关系是建立起来了，所以应该是qt 下不能发布消息，这个咋整
4.25
看到这界面，心情一下子轻松了，这“该死的”消息终于发送出去了！！！！！！！
整整两天的时间，我在找不能发布消息的原因。从莫名其妙、一头雾水（明明我都没动这部分代码，就突然出问题）到有点头绪，我一点点缩小范围，回退到几天前版本，没解决; qt新建一个test包，发布hello world消息，没成功; 测试publish和subscribe,确定只是不能subscribe。 重装ros, 没解决，排除ros的原因。 自己写一个talk和listen节点，可以发布和接收消息，进一步排除ros原因。
那么范围就缩小到qt 。我用qt重新建了一个工作空间，把几个包放进去，编译，运行，没解决，排除工作空间的问题。那就检查代码喽。网上搜到一个问题，也是说节点not publishing ,有回答说advertise话题后要sleep一会，等设置完毕，不能马上发布消息，于是ros::Duration(5).sleep() ，休眠5s， 没用; 有回答说可能订阅消息的回调函数阻塞了，不能回去publish，于是我把订阅的话题都取消了，也没用。于是又陷入了僵局，如果说代码出问题，那为什么还能接收话题消息呢，如果发布话题设置出错，那为何其他节点能订阅ur5_monitor 发布的消息呢（用rostopic info /command 查看到了其他节点已经订阅了消息）？很郁闷，用了这么多时间，问题还是没解决，很烦躁。
问题的解决也可以说是巧合，或者说源于我敏锐的洞察力。ur5_monitor发布话题/command, 然后我写了一个listener订阅这个话题。我在使用rosrun rqt_graph rqt_graph 查看个节点运行状况时，还没启动listener,不过启动了ur5_monitor, 整个rqt_graph的界面是空白的，没有ur5_monitor。我就觉得很奇怪，明明我已经将ur5_monitor连接到了ros master。 接着我启动了listener节点，刷新rqt_graph ,出现了ur5_monitor和listener这两个节点。于是我马上意识到是不是ur5_monitor连接ros_master有问题，因为只有别的和它有关系的节点启动时，它才能被rqt_graph发现。
 图1 单独启动ur5_monitor |
 图2 再启动listener |
于是我查看了初始化ur5_monitor节点的代码，连接需要master_url 和 host_url，这些在ui界面可以设置，也qt下ros例程包的部分，而且它还有另一种初始化方式，ros::init(init_argc,init_argv,"ur5_monitor");一般的ros 节点的initf方式也是这样。
既然节点初始化有问题，我就换了ros::init(init_argc,init_argv,“ur5_monitor”);来初始化，编译，运行，rqt_graph发现了ur5_monitor, 也成功发送出了消息。问题解决了，就是节点初始化出问题，但是前面那种初始化方式之前是可以了，不然qt也不会把它写成例程。那就说明还是我的系统有哪里出了问题，导致失败。
同时我发现了前面失败时在rqt_graph 上显示时前面多了一个前缀n__，而正常时则没有。前面失败时能接收到关节信息，那么这种状态下rqt_graph上是怎么显示的呢？
可以看到仍然带有前缀，但这时能接收到关节消息h。失败的那种初始化方式下，我试了，先启动ur5_monitor,再启动ur_driver, 不能接收到关节消息。必须先启动ur_driver, 再把ur5_monitor连接到master 才能接收到关节消息。这解答了我前面的疑惑：为什么不能发布消息却能订阅。原因就是，ur5_monitor连接到master后，马上就掉线了，即不能被其它节点发现，也不能通过master获得其它新节点上线的消息。为什么这么说呢？因为如果ur_driver后启动，ur5_monitor连接到master后没发现ur_driver，然后它就掉线了，即使ur_driver启动，它也不知道，所以没接收到关节消息。同样，发布消息也是，因为它掉线了，其它节点根本发现不了它， 当然接收不到它发布的消息。
虽然用了两天，但问题能解决还是开心的，最怕的就是花了时间却最终没能解决。但解决的过程还是很有收获了，磨练了耐心，关注细节，对ros机制理解更深了一点
享受不断学习进步的过程，不要太浮躁和带有功利性
关于启动一个节点时ros::init()的参数
首先，先了解一些关于ros 环境变量的知识
详细的可以看这里 http://www.mamicode.com/info-detail-915039.html
在ROS的环境变量中，需要在~/.bashrc中设置的并不多，常见的三个：
$ROS_MASTER_URI
$ROS_HOSTNAME
$ROS_IP
[URL]
统一资源定位符
[ROS_MASTER_URI]
是一个必需的设置来告诉节点在哪里定位master。应该设置master的XML-RPC URI。使用localhost时需要格外小心，因为远程启动节点可能会导致意想不到的结果。因为除了’localhost’(本地主机),ROS成员绑定到所有可用的网络接口，他不影响实际绑定地址。如果这个值设定为本地主机，ROS成员只绑定在环回接口。这将会阻止远程成员与本地成员交流。
[ROS_HOSTNAME]和[ROS_IP]
ROS_IP和ROS_HOSTNAME是可选的环境变量，用来设置ROS节点或工具的公开网地址。这两个选项是互斥的，如果两者都设置优先使用ROS_HOSTNAME。
如果你指定一个IP地址，使用ROS_IP；
如果制定一个主机名(a host name)，使用ROS_HOSTNAME。
当一个ROS成员报告URI给master或者其他成员，这个值(ROS_HOSTNAME或ROS_IP)就会被用到。
关于这些环境变量，都在./bashrc文件中配置
查看了环境变量，只有一个ROS_MASTER_URI，并且是使用localhost, 就是用本地主机名
 |
 |
https://blog.csdn.net/wanghuiquan0712/article/details/78014232
https://blog.csdn.net/wanghuiquan0712/article/details/78052093
qt 的例程init()默认采用的是这种重载void init(const M_string& remappings, const std::string& name, uint32_t options = 0);，但不知我的环境出了什么问题，突然就不能用了。
|
 |
4.28
运行了一下librealsense的measure例程，测量结果挺准的
cd ~/software/librealsense/build/examples/measure
./rs-measure
用opencv结合librealsense的代码显示了相机图像，但深度图没法显示
把相机拿起来后发现depth窗口里有东西闪了一下，于是晃了晃，发现其实是有图像的，但因为距离很近，所以很黑，对着远处的物体就有灰色的部分出现了。但前面的那张图是有拍到远处的东西的，因为超出了范围，所以无值，为0,显示为黑色
后面试着加个伪彩色处理，不然看不清
伪彩色处理
#bgr三色
cv::Mat img_color(depthmat.rows,depthmat.cols,CV_8UC3);
int tmp=0;
for (int y=0;y<depthmat.rows;y++)
{
for (int x=0;x<depthmat.cols;x++)
{
tmp = depthmat.at<unsigned short>(y,x);
img_color.at<Vec3b>(y,x)[0] = abs(255-tmp); //blue
img_color.at<Vec3b>(y,x)[1] = abs(127-tmp); //green
img_color.at<Vec3b>(y,x)[2] = abs( 0-tmp); //red
}
}
#彩虹图
// 红 255, 0, 0 255
// 橙 255, 127, 0 204
// 黄 255, 255, 0 153
// 绿 0, 255, 0 102
// 青 0, 255, 255 51
// 蓝 0, 0, 255 0
//转为彩虹图的具体算法，主要思路是把灰度图对应的0～255的数值分别转换成彩虹色：红、橙、黄、绿、青、蓝。
int tmp2=0;
#define IMG_B(img,y,x) img.at<Vec3b>(y,x)[0]
#define IMG_G(img,y,x) img.at<Vec3b>(y,x)[1]
#define IMG_R(img,y,x) img.at<Vec3b>(y,x)[2]
for (int y=0;y<depthmat.rows;y++)
{
for (int x=0;x<depthmat.cols;x++)
{
tmp2 = depthmat.at<ushort>(y,x);
if (tmp2 <= 51)
{
IMG_B(img_color,y,x) = 255;
IMG_G(img_color,y,x) = tmp2*5;
IMG_R(img_color,y,x) = 0;
}
else if (tmp2 <= 102)
{
tmp2-=51;
IMG_B(img_color,y,x) = 255-tmp2*5;
IMG_G(img_color,y,x) = 255;
IMG_R(img_color,y,x) = 0;
}
else if (tmp2 <= 153)
{
tmp2-=102;
IMG_B(img_color,y,x) = 0;
IMG_G(img_color,y,x) = 255;
IMG_R(img_color,y,x) = tmp2*5;
}
else if (tmp2 <= 204)
{
tmp2-=153;
IMG_B(img_color,y,x) = 0;
IMG_G(img_color,y,x) = 255-uchar(128.0*tmp2/51.0+0.5);
IMG_R(img_color,y,x) = 255;
}
else
{
tmp2-=204;
IMG_B(img_color,y,x) = 0;
IMG_G(img_color,y,x) = 127-uchar(127.0*tmp2/51.0+0.5);
IMG_R(img_color,y,x) = 255;
}
}
}
 bgr三色 |
 彩虹图，分层更多，但还是有点奇怪，只有红橙蓝三色 |
 |
加载灰度图时，图片存储格式位数为16位，而opencv默认为8位读取， 所以只获取了后面8位的数，所以很暗。
而转换成彩虹图时，由于是16位，数值绝大多全部大于255或为0，全都聚集在第一个和最后一个区间，故图像中只有红橙和蓝色，tmp2 = depthmat.at<ushort>(y,x)/32;，将数值除以16后再上色，即将16位数据右移了4位，效果好了很多。
左边 是 cv::imshow(“depth”,depthmat*8); 相当于把数值变大了八倍，图像亮了很多。
librealsense相关 http://www.pianshen.com/article/92562613/
接下来做这个，深度图像和彩色图像对齐 https://blog.csdn.net/jay463261929/article/details/53582800
4.30深度图像和彩色图像对齐
OpenCV对16bit的视频没有保存的机制，所以只能把原始深度数据保存为png格式
基础知识
世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换
https://blog.csdn.net/lyhbkz/article/details/82254069
相机标定——图像坐标与世界坐标转换
https://blog.csdn.net/Kalenee/article/details/80659489
D435深度图片对齐到彩色图片-代码实现
https://blog.csdn.net/dieju8330/article/details/85300976
https://blog.csdn.net/dieju8330/article/details/85346454
librealsense自己有一个函数可以把深度图转成伪彩色，感觉效果更好一点
//深度图像颜色map
rs2::colorizer c; // Helper to colorize depth images
rs2::frame depth_frame_show = data.get_depth_frame().apply_filter(c);
 |
 冷色调光 |
 暖色调光 |
5.2
深度图和rgb图对齐后，计算出圆心周围50×50范围depth数据的平均值作为球到摄像头距离(单位:mm)。先做开环的抓取，获取球的世界坐标后，直接规划机械臂到目标点进行抓取。
 |
5.4
前面的识别红球是在rgb空间下进行的，用红色通道减去其它两个通道来去掉非红色的物体，但是受光线影响很大且能识别的颜色颜色单一。
// 分离红色通道图像
Mat channel[3];
int g_nHm = 20; // 要减去通道的比例
split(img, channel);
channel[0] = channel[0].mul(.1*g_nHm); // B (mul: per-element matrix multiplication)
channel[1] = channel[1].mul(.1*g_nHm); // G
channel[2] = channel[2] - channel[0] - channel[1]; // R
channel[2] = 3 * channel[2];
imshow("red", channel[2]);
下面准备转到hsv空间下进行识别。
一般对颜色空间的图像进行有效处理都是在HSV空间进行的，然后对于基本色中对应的HSV分量需要给定一个严格的范围，下面是通过实验计算的模糊范围（准确的范围在网上都没有给出）。
色调H: 0 - 180 （Hue）
饱和度S: 0 - 255 (Saturation，下面的chroma也是饱和度的意思)
亮度V: 0 - 255 （Value）
此处把部分红色归为紫色范围：
参考这篇博客完成了hsv空间下的指定阈值二值化
OpenCV——基于颜色的物体检测系统
https://www.cnblogs.com/wangxinyu0628/p/5928824.html
使用下面的函数将rgb图像转成hsv图像
cv::cvtColor(image, hsv_image, CV_BGR2HSV);
转换完成后分离出3个通道
vector<cv::Mat> channels;
cv::split(hsv_image, channels);
然后遍历整幅图像的像素点，符合红色的hsv范围的点设为255白色，否则为0黑色，输出二值化图像
由于霍夫圆检测对噪点很敏感，所以必须对二值化图像进行高斯滤波，平滑图像，减少错误的识别
GaussianBlur(dstImg, //源图像 channels不限，各通道单独处理；
dstImg1, //目标图像 与原始图像size和type一致
Size(9, 9), //高斯核大小，必须为奇数
3, //高斯核在x方向的标准差,值越大，处理结果越平滑（模糊）
3); //高斯核在y方向的标准差
 高斯滤波前后对比 |
霍夫圆检测 输入图像须为8位灰度
vector<Vec3f> circles; // 3通道float型向量
HoughCircles(dstImg1, //需为8位的灰度单通道图像
circles,
CV_HOUGH_GRADIENT, //检测圆的方法，OpenCV2.*.*版本之中只有霍夫梯度法
1, //累加器图像的分辨率,增大则分辨率变小
dstImg1.rows/3, //两个圆之间的距离的最小距离
200, //canny算法的阈值上限，下限为一半
20, //累加器的阀值,圆心检测的阈值（若累计阈值大于此阈值则认为是圆心），
//如果这个阈值设置的越小，更多错误的圆容易被检测到。
50, 500 //最小圆半径和最大圆半径
);
HSV空间下光照对检测结果的影响
 冷色调的光 |
 暖色调的光 |
可以看到暖色调的光下球体上出现了很多黑色的噪点。这是因为红色球体吸收的红光，反射其它光，现在光源中是暖色调的光，其中红色分量多，其它分量少，所以球体反射的光也少了，导致二值化时出现了黑点。而且图片球体外的区域出现了一些白色的小噪点，导致霍夫识别检测出了错误的一些圆。
为了排除错误的圆，要对计算圆内的白色像素占整个圆的像素总数的比值，选出白色比值最大的圆作为正确的圆，用绿色显示
//针对圆，计算比率
int flag = 0, count = 0;
float rate = 0,max=0;
for (int i = 0; i < circles.size(); i++)
{
count = 0;
rate = 0;
int a = circles[i][0];
int b = circles[i][1];
int r = circles[i][2];
//cout << "center:" << a <<","<< b << "r:" << r << endl;
for (int m= a - r; m<=a + r; m++)
{
//从包含圆的最小正方形出发
if (m<0)
m=0;
uchar* p = img.ptr<uchar>(m);
for (int n= b - r; n<=b + r; n++)
{
if (n<0)
n=0;
int tem = (m - a)*(m - a) + (n - b)*(n- b);
if (tem < r*r) //在圆内部
{
if (p[n] == 255)count++; //白色像素点
}
}
}
rate = (float)count/(3.14*r*r);//注意括号！！！
if (rate > max) { max = rate; flag = i; return flag;} //记住比率最大的圆
}
 选出了正确的圆 |
 将二值化图像作为遮罩作用在原图上 |
物体的深度值只有在摄像头正对物体且到镜头近处没有遮挡时才准确
 俯视图，准确，摄像头无遮挡 |
 |
 45度向下视图，数值错误，摄像头下方空间被桌面遮挡(图像下方有红色区域) |
5.5
写了一个继承QSlider的myslider类，重绘了外观，实现调色条的作用。在qt designer中将滑条promote为myslider就可以了。
 |
 |
qt上位机制作调色板，参考这个博客继续制作饱和度和亮度调色板
https://www.cnblogs.com/hellovenus/p/5449131.html
QColor中 H: 0-359 S:0-255 V:0-255
5.6&&5.7
完成了调色板的制作，可以通过上位机选择两个hsv颜色作为二值化的阈值，ur_monitor通过请求setHsvColor服务完成hsv值的设置`。这两天真的很辛苦，累得头疼，排除了很多困难，把调色板效果做到最好。真棒！！！
 |
 |
5.8
完善了开启图像窗口的部分，优化了界面
摄像机的旋转平移属于外参，用于描述相机在静态场景下相机的运动，或者在相机固定时，运动物体的刚性运动
下面开始学习机械臂的手眼标定
相机内参： 描述相机坐标系的点到像素坐标系的变换。、
相机外参： 世界坐标系到相机坐标系的变换。摄像机的旋转平移属于外参，用于描述相机在静态场景下相机的运动，或者在相机固定时，运动物体的刚性运动
从像素坐标到世界坐标
手眼标定之基本原理
https://www.jianshu.com/p/3e302adc7aa5
https://www.jianshu.com/p/ea4de928eb23
https://www.wengbi.com/thread_77528_1.html
easy_handeye
github https://github.com/IFL-CAMP/easy_handeye
https://blog.csdn.net/zhang970187013/article/details/81098175
UR5、Kinect2手眼标定总结
https://blog.csdn.net/weixin_40799950/article/details/82594537
eye in hand
Eye-in-hand和Eye-to-hand问题求解和实验
https://blog.csdn.net/weixin_36444883/article/details/84867362
使用easy_handeye标定后会得到一个camera_link和机械臂末端间的坐标变换矩阵,
ceH表示摄像机（camera）坐标系相对于机器人末端（end）TCP坐标系的齐次变换矩阵
将图像坐标系变换到相机坐标系，接着再左乘相机与机器臂末端的变换矩阵，即可得到物体在机器臂末端tool坐标系下的位姿。
使用easy_handeye时要写一个launch文件，发现有些还看不懂
重新学习ROS 命名空间，参数，参数服务器，重映射！！！
http://chev.me/arucogen/ 生成的aruco的marker的大小及编号，注意选择
写了一个launch文件，启动相机sr300, aruco_tracker, ur5机械臂和easy_handeye，来进行手眼标定roslaunch ur_monitor eyeinhand.launch。
运行后报错一些文件没有权限，因为vision_visp文件夹被设为只读chmod -R 700 vision_visp/ (Document/)
-R参数是递归 处理目录下的所有文件以及子文件夹
700是变更后的权限表示（只有所有者有读和写以及执行的权限）
Document/ 是需要执行的目录
launch文件里的话题重映射写的不对，改成sr300发布的话题名称后可以显示aruco识别的结果
<node name="aruco_tracker" pkg="aruco_ros" type="single">
<remap from="/camera_info" to="/camera/color/camera_info" />
<remap from="/image" to="/camera/color/image_raw" />
点take_sample时有时会卡死，报错如下
Error processing request: Lookup would require extrapolation at time 1557329094.403053999, but only time 1557329092.472987890 is in the buffer, when looking up transform from frame [wrist_3_link] to frame [base_link]
由于tf的会把监听的内容存放到一个缓存中，然后再读取相关的内容，而这个过程可能会有几毫秒的延迟，也就是，tf的监听器并不能监听到“现在”的变换
5.9
今天成功完成了一次标定，还是有检测不到相机或take example 时因为获取tf时延报错卡死的情况，不过多重试几次还是可以的。下面就是获取到的结果
/*******************
* 相机坐标系到base的转移矩阵
translation:
x: -0.220035840896
y: 0.094281833652
z: 0.392433641299
rotation: //四元数
x: 0.654817936692
y: 0.293319224979
z: 0.00315024835412
w: 0.696539573879
**********************/
由四元数(w,x,y,z)和平移向量计算变换矩阵，可以使用Egien的库函数。计算结果如下：
识别出球后，从图像坐标系转移到相机坐标系可以有realsense的库完成，再左乘上面得到的相机坐标系到tool(电爪的ee_link)的转移矩阵 (前面标定时launch中写的是wrist3_link, tf只发布到wrist3的信息) 即可得到球在tool坐标系下的坐标，这个坐标可以作为在base坐标系下需要移动的增量, 通过笛卡尔空间规划控制机械臂末端电爪到达该点（抓取时wrist3平行z轴，且电爪与wrist3刚性连接在一起，在z方向减去电爪高度），完成抓取。
还有就是球的识别只在30cm有效，得改进一下，起码50cm内能准确识别。找到原因了，霍夫圆检测的最小圆半径是50，设为30后能检测到50cm处的圆
上位机控制关节运动时滑条的拖动不是很合理，而且拖动时精度不够，很容易一下旋转很多。要改进滚轮滑动也能移动。
图像处理
二值化后先进行腐蚀操作，可以看到大部分噪点和小块的白色都被腐蚀掉了，然后进行膨胀操作，对目标图像复原，效果很好
但是，如果因为光线原因（目标背光，偏暗）而导致二值化效果不好，但还是可以检测出圆，如果进行腐蚀和膨胀反而会使目标图像丢失。
那么先膨胀再腐蚀呢？虽然球体白色部分多了一些，但没消除噪点
前面上面的卷积核是10×10，下面是30×30，卷积核越大，效果越强
5.14
额,今天摄像头装在支架上又进行了一次标定，但转换出来的wrist_3_link坐标系下的坐标值很离谱, 冷静分析后发现转换矩阵就很离谱，旋转和平移的量都很大(上一次得到的那个也是)，于是就怀疑是easy_handeye 出了问题，检查了launch文件的设置，没看出什么问题。于是到github看说明。
分为eye_in_hand和eye_on_base两种情况，相机在机械臂上时eye_on_hand 参数要置true！！！！
就是这里我设置错了。因为看到这篇博客 https://www.jianshu.com/p/ea4de928eb23 说有eye_in_hand是相机在机械臂上，eye_on_hand是相机在机械臂外(尽管我觉得in和on 没什么太大区别，但没深究)，所以我就把eye_on_hand参数置false了。。。所以标定时easy_handeye是认为相机在机械臂外得到的变换矩阵，结果肯定不对。还是github 上作者的说明最靠谱，虽然是英文的，也得耐心看。
<arg name="robot_effector_frame" value="ee_link" /> <!-- 机械臂末端执行机构-->
static_transform_publisher
static_transform_publisher工具的功能是发布两个参考系之间的静态坐标变换，两个参考系一般不发生相对位置变化。命令的格式如下：
static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms
以上两种命令格式，需要设置坐标的偏移和旋转参数，偏移参数都使用相对于xyz三轴的坐标位移，而旋转参数第一种命令格式使用以弧度为单位的 yaw/pitch/roll三个角度（yaw是围绕x轴旋转的偏航角，pitch是围绕y轴旋转的俯仰角，roll是围绕z轴旋转的翻滚角），而第二种命令格式使用四元数表达旋转角度。发布频率以ms为单位，一般100ms比较合适。
该命令不仅可以在终端中使用，还可以在launch文件中使用，使用方式如下：
<launch>
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1 100" />
</launch>
来源：CSDN
作者：eye998
链接：https://blog.csdn.net/eye998/article/details/89458316