显微操作中操作器末端的姿态评估

实验室的一部分个人工作，之前发过一篇这方面的博客关于探针末端跟踪的前期尝试 。哇塞，发现整整过去一年啦。。。。_{生如逆旅单行道，哪有岁月可回头。对不起，我没能活成你想要的样子。}哈哈，下面正式切入主题。

问题背景介绍

上次也介绍了，但好像没介绍清楚。再来一遍，看图。

显微镜下就是这么一个操作的过程：通过微操作器末端（探针）的一系列移动和按压式操作捡起这些圆环状的物体（donut）。_{捡这破玩意干啥？不要问我。}目前人们是怎么完成这一任务的？显微镜_{（没错，是显微镜。这一系列操作是在显微镜下完成的，所以叫“显微操作”嘛。）}物镜处的摄像头接到电脑上，实验员一边盯着屏幕，一边晃动着手中的鼠标来控制探针的移动，从而完成任务。
问题来了，现在我们想要自动化地完成这个任务，怎么办？仅从感知角度讲，我们需要环境信息：探针方向，探针的末端位置，donut的位置。简单来说就是这些。今天我们要解决探针方向和末端位置这个问题。

解决方案

上述提出的问题难度几颗星？这得看你具体操作场景的复杂程度了，比如探针末端是否遮挡，背景是否干净，donut的各种姿态及重叠是否考虑_{立着的donut是在液体中漂起来了}。假如以真实操作环境作为参照，其实没有那么简单。下面说解决方案。
1. 这里我们先跳过最最重要的一步！！！，即假设我们已经知道了探针的大致位置，如下图所示。

这一步怎么做到的？一句话解释，基于深度学习的目标检测算法YOLO，其实本文的工作接的是这一篇博文YOLO下一步：输出预测boundingbox做进一步处理 。_{年前就做了，拖到现在。。。}
接下来，我们就在这个ROI区域里找出探针方向和末端位置。
2.在ROI区域里找出探针轮廓

3.应用hough transform用直线拟合轮廓

4.根据拟合的直线算出探针的方向。
这里需要注意，上一步一般会拟合出不止两条直线。我们可以简单地对所有直线求平均来得到方向线，也可以使用NMS（非极大值抑制）来找出两侧的两条最长的直线作为轮廓直线然后在对其求平均得出方向线。

5. 求出探针末端点位置
我们直接将探针轮廓和方向线的交点作为末端点位置。
当然这样立马有一个问题摆在眼前，末端被遮挡了怎么办。首先，经实际测试发现：即使遮挡时也不会错很多，而且一不遮挡立马就回来了。当然，因为在视频序列里探针是连续移动的，比如加个卡尔曼滤波就可以了。_{如果你有别的好想法赶紧告诉我。}

6.完成
如图所示，结果就是这样。

好，大致过程就是这样了。欢迎提出别的想法。

代码实现

目录树：

    -main.cpp
    -inData.cpp   //这是输入数据的类，也就是程序开头那个boundingBox信息数据
    -inData.h
    -tip.cpp  //关于探针的类
    -tip.h
    -images  //存放原始图片的文件夹
    -post_images  //存放处理后图片的文件夹

main.cpp

#include <opencv2/opencv.hpp>
#include <iostream>
#include <cstdio>
#include <vector>
#include <cmath>
#include "tip.h"
#include "inData.h"
using namespace cv;
using namespace std;

int frames = 1590;  //总共帧数
char coordinateFile[] = "tip_coordinate.txt";
char imageFolder[] = "images";

struct {
    bool operator()(vector<Point> a, vector<Point> b){
        return a.size() > b.size();
    }
} contourGreater;


int main() {
    inData singleTip(coordinateFile, imageFolder);
    for (int frame = 0; frame < frames; frame++) {  //914帧之后会出现问题
        Mat srcImage = singleTip.readImg(frame); //读入灰度图
        Rect tipRoi = singleTip.readBox(frame);

        Mat biImage;
        threshold(srcImage, biImage, 200, 255, CV_THRESH_BINARY_INV); //对整张图进行二值化处理
        Mat biColorImage = biImage.clone();
        cvtColor(biImage, biImage, CV_BGR2GRAY);

        Mat imageROI = biImage(tipRoi);
        vector<vector<Point> > contours;
        vector<Vec4i> hierarchy;
        findContours(imageROI, contours, hierarchy, CV_RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);     
        if (contours.size() > 1) {
            sort(contours.begin(), contours.end(), contourGreater); //找出size最大的轮廓
        }
        Mat edge_image(imageROI.rows, imageROI.cols, CV_8UC1, Scalar(0, 0, 0));
        drawContours(edge_image, contours, 0, Scalar(255, 255, 255), 1, 8, hierarchy, 1);//绘制轮廓

        tip alpha(edge_image);
        Point st, ed;
        alpha.accessPose(st, ed);
        arrowedLine(srcImage, st + Point(tipRoi.x, tipRoi.y), ed + Point(tipRoi.x, tipRoi.y), Scalar(0, 0, 255), 2, 8, 0, 0.1);
//      circle(srcImage, alpha.accessEndPoint() + Point(tipRoi.x, tipRoi.y), 2, Scalar(0, 255, 0));  //画出其交点
        imshow("【效果图】", srcImage);
        char image_name[50];
        sprintf(image_name, "post_images/A_%08d.jpg", frame);
        imwrite(image_name, srcImage);
        waitKey(3);
    }

    return 0;
}

inData,cpp

#include "inData.h"

inData::inData(char coordinateFile[], char imageFolder[])
{
    read_coordinate(coordinateFile);
    strcpy_s(folder, imageFolder);
}


inData::~inData()
{
}

int inData::read_coordinate(char fileName[]) {
    ifstream fin(fileName);
    vector<int> tipBox(4);
    char buffer[256];
    int frames=0;
    while (fin.getline(buffer, 256, '\n')) {
        istringstream iss(buffer);
//      iss >> tipBox[0]; iss >> tipBox[1]; iss >> tipBox[2]; iss >> tipBox[3];
        iss >> tipBox[0] >> tipBox[1] >> tipBox[2] >> tipBox[3];
        tipsBox.push_back(tipBox);
        frames++;
    }
    return frames;
}

Mat inData::readImg(int frame)
{
    char image_name[50];
    sprintf_s(image_name, "%s/A_%08d.jpg", folder, frame + 1);
    return imread(image_name);
}
Rect inData::readBox(int frame)
{
    return Rect(tipsBox[frame][0], tipsBox[frame][1], tipsBox[frame][2]- tipsBox[frame][0], tipsBox[frame][3]- tipsBox[frame][1]);
}

inData.h

#pragma once
#include <fstream> 
#include <sstream>
#include <cstring>
#include<vector>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;

class inData
{
public:
    inData(char coordinateFile[], char imageFolder[]);
    ~inData();
    Mat readImg(int frame);
    Rect readBox(int frame);
private:
    int inData::read_coordinate(char fileName[]);
    vector<vector<int> > tipsBox;
    char folder[50];
};

tip.cpp

#include "tip.h"

tip::tip(Mat image)
{
    contourLines(image, lineA, lineB);
    theta = (lineA[1] + lineB[1]) / 2;
    Point2f p = intersection(lineA, lineB);
    rho = calcRho(p);
    angle = calcAngle(p, Point(image.cols/2, image.rows/2));
    endPoint=calcEndPoint(getContour(image), Point(image.cols / 2, image.rows / 2));
}

tip::~tip()
{
}

Point tip::accessEndPoint() {
    return endPoint;
}

int tip::accessPose(Point& a, Point& b) {
    b = endPoint;
    float g = angle / 180 * CV_PI;
    a.x = b.x - 200 * cos(g);
    a.y = b.y + 200 * sin(g);
    /*
    if (angle >= 0 && angle<90) {   
        a.x = b.x - 200*cos(g);
        a.y = b.y + 200*sin(g);

    }
    else if (angle >= 90 && angle<180) {
        a.x = b.x - 200 * cos(g);
        a.y = b.y + 200 * sin(g);
    }
    else if (angle >= 180 && angle<270) {
        a.x = b.x - 200 * cos(g);
        a.y = b.y + 200 * sin(g);
    }
    else {
        a.x = b.x -+ 200 * cos(g);
        a.y = b.y - 200 * sin(g);
    }
    */
    return 0;
}

float tip::calcRho(Point2f p)
{
    float sm = sin(theta); float cm = cos(theta);
    return p.x*cm + p.y*sm;
}

float tip::calcAngle(Point2f p, Point center) {
    if (p.x > center.x && p.y <= center.y) { //第一象限
         return 90 - theta * 180 / CV_PI;
    }
    else if (p.x <= center.x && p.y < center.y) {  //第二象限
        return 270 - theta * 180 / CV_PI;
    }
    else if (p.x < center.x && p.y >= center.y) {  //第三象限
        return 270 - theta * 180 / CV_PI;
    }
    else {
        return 450 - theta * 180 / CV_PI;
    }
}

Point2f tip::intersection(Vec2f a, Vec2f b) {

    float sa = sin(a[1]); float ca = cos(a[1]); float sb = sin(b[1]); float cb = cos(b[1]); 
    float denominator = (ca*sb - cb*sa);
    float x = (a[0] * sb - b[0] * sa) / denominator;
    float y = (b[0] * ca - a[0] * cb) / denominator;
    return Point2f(x, y);
}

struct {
    bool operator()(Vec2f a, Vec2f b)
    {
        return a[1] < b[1];
    }
} lineLess;
int tip::contourLines(Mat image, Vec2f& a, Vec2f& b) {
    vector<Vec2f> lines;
    HoughLines(image, lines, 1, CV_PI / 180, 80);
/*
    Mat tmg;
    cvtColor(image, tmg, CV_GRAY2BGR);
    for (size_t i = 0; i < lines.size(); i++) {
        float rho = lines[i][0], theta = lines[i][1];
        Point pt1, pt2;
        double a = cos(theta), b = sin(theta);
        double x0 = a*rho, y0 = b*rho;
        pt1.x = x0 + 1000 * (-b);
        pt1.y = y0 + 1000 * (a);
        pt2.x = x0 - 1000 * (-b);
        pt2.y = y0 - 1000 * (a);
        line(tmg,pt1, pt2 , Scalar(0, 0, 255));
        //验证找到的几条hough直线
    }
    imshow("【临时图】", tmg);
*/

//  if (lines.size() < 2) return 0; //有时候检测不到两条比较好的直线，此时使用上一次的轮廓线
    sort(lines.begin(), lines.end(), lineLess);  //貌似不用排序，其本来就是按theta从小到大排列的
    a = lines.front();
    b = lines.back();
    return 0;
}

Point tip::calcEndPoint(vector<Point> contour, Point center) {
    for (int k = 0; k < contour.size(); k++) {
        int dis = cos(theta)*contour[k].x + sin(theta)*contour[k].y - rho;
        if (dis == 0) {
            if (angle>=0 && angle<90){
                if (contour[k].x > center.x && contour[k].y <= center.y) {
                    return contour[k];
                }
            }
            else if (angle >= 90 && angle<180) {
                if (contour[k].x <= center.x && contour[k].y < center.y) {
                    return contour[k];
                }
            }
            else if (angle >= 180 && angle<270) {
                if (contour[k].x < center.x && contour[k].y >= center.y) {
                    return contour[k];
                }
            }
            else {
                if (contour[k].x >= center.x && contour[k].y > center.y) {
                    return contour[k];
                }
            }
        }
    }       
}

vector<Point> tip::getContour(Mat img) {
    vector<Point> contour;
    int ncols = img.cols;
    int nrows = img.rows;
    for (int x = 0; x < ncols; x++) {
        for (int y = 0; y < nrows; y++) {
            if (img.at<uchar>(y, x) == 255) {
                contour.push_back(Point(x, y));
            }
        }
    }
    return contour;
}

tip.h

#pragma once
#include <opencv2/opencv.hpp>
#include<vector>
using namespace cv;
using namespace std;

class tip
{
public:
    tip(Mat image);
    ~tip();
    Point accessEndPoint();
    int tip::accessPose(Point& a, Point& b);
private:
    Point2f intersection(Vec2f a, Vec2f b);
    Vec2f lineA;   //轮廓的两条边界拟合线
    Vec2f lineB;
    float calcRho(Point2f p);
    float calcAngle(Point2f p, Point center);
    int contourLines(Mat image, Vec2f& a, Vec2f& b);
    Point calcEndPoint(vector<Point> contour, Point center);
    vector<Point> getContour(Mat img);
    float rho;    
    float theta;
    float angle;
    Point endPoint;
};