上文完成了轨迹生成和轨迹跟踪的任务，本次在上次的基础上实现了识别圆环真实圆心在世界坐标系下的位置，并用当前圆环圆心的真实位置和后面几个圆环圆心的大致位置重新生成一条轨迹，每一次识别一个圆环中心就重新规划一次轨迹，最终完成了穿圈任务。
代码：https://github.com/USE-jx/Robomaster-uav-competition
代码能力不足，我看了很多ROS最佳实践了，也参考了很多好的开源代码怎么写，但是模块化可能不是很好，见谅。

1 识别圆环圆心坐标转到世界坐标系下

图像识别我没咋接触过，要是不是为了完成任务我是没多大兴趣的，就是学视觉slam时学过一点。做的时候一直很抵触，学新的东西很痛苦，学不喜欢的新东西更痛苦，但是还是的想办法解决，最后果然没做好，就因为这个识别环的能力不行导致飞机速度过快就可能识别失败，看来想做好点，每个环节都不能拉胯。

1.1 识别图像中的圆环

首先可以看到圆环是黄色的，所以参考这个OpenCV: Thresholding Operations using inRange官网教程可以拉进度条最后提取到只有圆环的像素坐标，如下面视频所示，
视频链接

1.2 找圆心

1.2.1 彩色图找圆心

参考这个OpenCV: Creating Bounding boxes and circles for contours官网教程可以用上面得到的只有圆环像素的图像先获取轮廓，确定圆心和半径并画在原图上，如下面视频所示，
视频链接
这个效果好的我都以为识别圆心已经结束了，直到我用上面调好的hsv参数在仿真环境一跑，我发现周围环境的小黄花都识别了进去，每个小点也都是圆心，一张图无数个圆心，这可如何是好啊！
继续看看官方教程，看看有没有啥可用的吧！在官网又发现了OpenCV: Hough Circle Transform，这个可以直接在图像中找到圆，并且得到圆心，所以就把找轮廓换成了霍夫圆。但是还是不行，小黄花那个小点也会被检测成圆，小黄花这个根本问题没有得到解决。这个时候我已经开始用深度图加霍夫圆测试了。
后来我还是不死心，又找了找资料，发现有一个contourArea东西，找到的轮廓的面积，我灵机一动，可以用这个东西过滤一下小黄花呀！测试了几次找到了一个合适的面积确实都过滤掉了，只剩圆环的中心了。但是，还是有问题，我发现有时候无人机在离环很近的时候会把半个环轮廓括起来，因为离得近所以面积也大于过滤面积，这时就会又发现一个圆心，真是太糟心了，情况如下图所示：

对这个彩色图解决办法我真的无奈了，一个又一个问题，还有最后一个红色环没有解决呢！最后转到世界坐标系需要深度信息，还得用深度图，我一想既然这样那还不如直接用深度图得了，我对比过两个识别圆心的效果，基本一样，所以决定弃用彩色图，采用深度图解决方案。我看别人都是用彩色图解决的，可能opencv还有我没发现的武器，但我真没兴趣再找了。

1.2.2 深度图找圆心

深度图不能直接用，需要把深度图转换成灰度图。一般做法是把深度值除以最大深度（这个是10m）再乘255，这就把深度值转换成了无符号八位的像素值。然后用深度图通过霍夫圆检测很容易就获得圆心的像素坐标了，此时又出现了一个新问题，怎么获取圆心的深度信息，圆心那没有东西，红外光直接就穿过去了。那可以用和圆环深度的距离代替一下啊，我感觉可以。

1.2.3 获取相机系下的圆心深度值

这就不得不了解一下红外光测距原理了，我当时就特别困惑假如相机正对着一面墙，每个像素点的深度是一样的吗？如下图所示，
测量的深度到底是PO还是AO。
最后我看视觉slam14讲对深度的用法，知道了测量的深度其实是AO，也就是正对一面墙深度是一样的。
知道这点以后，如果正对着圆环那么圆环平面的深度和圆心深度应该是一样的，所以可以随便取环上一个点的深度。
但是，如果是椭圆视角，从霍夫圆上取一点可能不在真环上，类似下面图片所示，

不在环上的话检测到的深度就是0了，所以我取了和圆心半径距离上下左右四个点，取最大值作为深度，还是可能出现深度为0，后来我看了看14讲，想到了可以遍历圆环外接正方形这个区域的像素，像素值为零的抛出去，最后取一下平均值，终于把问题解决了，得到了圆心的大致深度，太不容易了。
但是现在还有一个问题是，图片一直在传回来，收到一张图片就有一个圆心坐标，识别到一个环就得到一堆圆心坐标，怎么能只取第一个呢？这个问题很困扰我，晚上都失眠了。但是没白失眠，最后还是想出了解决办法。这是一个设一个标志位和return结合的标准问题，设置一个得到圆心的flag，flag置一后函数内开始就return，这样后边的图片就不会接收了。但是标志位一直为1的话后边圆都识别不到了，所以需要穿过一个环就把标志位置零。这时需要一个定时器，每隔0.5s检测一下和圆心的距离，小于一定值就判定已经穿过当前环，此时把标志位置零即可，最后终于实现了每个环圆心像素坐标只获得一次，深度值一次。几个月前我都想不到我能如此灵活地编程解决问题，于是兴冲冲地发了一个视频，如下，
视频链接
上述内容程序如下

void TrajectoryReplanNode::getCircleCenter(const ros::TimerEvent &e) {
    Eigen::Vector3d waypoint(waypoints_(row_idx_,0), waypoints_(row_idx_,1), waypoints_(row_idx_,2));
    double d = (waypoint - odom_pos_).norm();
    //ROS_INFO("dis to goal: %f", d);
    //ROS_INFO("goal : %f %f %f", waypoints_(row_idx_,0), waypoints_(row_idx_,1), waypoints_(row_idx_,2));
    if (d < 2.5) { //和他给的圆心距离小于2.5m就算通过了
        ++row_idx_;
        got_circle_flag_ = false;
        if (row_idx_ == waypoint_num_) {
            --row_idx_;
        }
    }
}
void TrajectoryReplanNode::depthImageCallback(const sensor_msgs::ImageConstPtr& msg) {
    if (got_circle_flag_) return;  //只接收一次

    depth_ptr_ = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    Mat depth = depth_ptr_->image;
    //cout << depth << endl;
    //cout << "rowsandcols:" << depth.rows << " "<< depth.cols << endl;
    Mat gray (depth.rows, depth.cols, CV_8UC1);
    Mat mask = Mat::zeros(depth.size(), CV_8UC1);

    for (int i = 0; i < depth.rows; ++i) {
        for (int j = 0; j < depth.cols; ++j) {
            uchar d = depth.at<float>(i,j) /10 * 255;
            gray.at<uchar>(i,j) = d;
            //cout << depth.at<float>(i,j) << endl; 
        }
    }
    medianBlur(gray, gray, 5);
    vector<Vec3f> circles;
    HoughCircles(gray, circles, HOUGH_GRADIENT, 1,
                50,80,28,15,30);
    //cout << circles.size() << endl;
    if (circles.size() == 1) {
        got_circle_flag_ = true;
        for (int i = 0; i < circles.size(); ++i) {
            Vec3f c = circles[i];
            Point center = Point(c[0],c[1]);      
            //扫描圆环外接矩行的像素深度，把深度不为0的像素深度加和取平均数
            float sum = 0;
            int count = 0;
            for (int v = c[1] - c[2] - 5; v < c[1] + c[2] + 5; ++v) {
                for (int u = c[0] - c[2] - 5; u < c[0] + c[2] + 5; ++u) {
                    float d = depth.ptr<float>(v)[u];
                    if (d == 0) continue;
                    sum += d;
                    ++count;
                }
            }
            double depth5 = sum / count;
                  
            circle(gray,center, 1, Scalar(255,255,255),1,LINE_AA);
            int radius = c[2];
            circle(gray, center, radius, Scalar(255,255,255),5,LINE_AA);
        }
    }
    imshow("depth2gray", gray);
    cv::waitKey(1);
}

1.3 圆心像素坐标和深度值转到世界坐标下

我本来以为我很熟悉，但是这部分又一次让我彻夜难眠了。
像素坐标转相机系下的坐标很容易，根据成像原理（如下图所示）很容易得到下列关系式，


$P=inv(K)\ast Z\ast [u,v,1{]}^T$
得到相机系下的坐标后，还需要相机系和机体系的一个变换，相机在机体前方0.26米，相机系的Z轴和机体系的X轴方向一致，相机系的Y轴和机体系的Z轴方向一致，相机系的X轴和机体系的Y轴方向一致，所以两个坐标系之间有平移和旋转关系。用$R_{cb}$ 表示坐标系相机系相对于机体系的旋转，$t_{cb}$ 表示相机系相对于机体系的平移，关系如下图所示，

机体系和世界系也类似，有一定的平移和旋转，机体系下点的坐标转换到世界系下点的坐标，飞机的姿态可以获取可以变成旋转矩阵，这个旋转矩阵表示了机体系三个轴相对于世界系三个轴旋转了多少，用$R_{bw}$ 表示坐标系机体系相对于世界系的旋转，$t_{bw}$ 表示机体系相对于世界系的平移，则这两个坐标系的点的坐标关系如下图所示，

我被难住的点在于我把坐标系的变换和两个坐标系内的坐标变换整混了，以至于我都怀疑十四讲代码是不是写错了，最后发现确实是错了，我错了。。。
下面是像素坐标和深度值转到世界系下的坐标程序，

//因为不长，所以写成了内联函数
//输入是用eigen三维向量存储的像素坐标和深度，输出是三维世界系下的坐标
	inline Eigen::Vector3d transformPixel2World(const Eigen::Vector3d &pixel_and_depth) {
        Eigen::Vector3d point_p (pixel_and_depth(0)*pixel_and_depth(2),
                                 pixel_and_depth(1)*pixel_and_depth(2),
                                 pixel_and_depth(2));
        //cout << "ceicle Center in pixel:" << point_p.transpose() << endl;                         
        Eigen::Matrix3d K;
        K << 160.0, 0.0, 160.0, 0.0, 160.0, 120.0, 0.0, 0.0, 1.0;
        Eigen::Vector3d point_c = K.inverse() * point_p;
        //cout << "ceicle Center in camera:" << point_c.transpose() << endl;
    	
        //R_c_b和t_c_b是相机系在机体系下的位姿
        Eigen::Matrix3d R_c_b;
        R_c_b << 0, 0, 1, 1, 0, 0, 0, 1, 0;
        Eigen::Vector3d t_c_b (0.26, 0, 0);
        Eigen::Vector3d point_b = R_c_b * point_c + t_c_b;
        //cout << "ceicle Center in body:" << point_b.transpose() << endl;
        
        //R_b_w和t_b_w是机体系在世界系下的位姿
        Eigen::Matrix3d R_b_w = odom_orient_.normalized().toRotationMatrix();
        Eigen::Vector3d t_b_w = odom_pos_;
        Eigen::Vector3d point_w = R_b_w * point_b + t_b_w;
        //cout << "uav current pos:" << t_b_w.transpose() << endl;
        
        //这个把R和t表示成了变换矩阵，也可以
        // Eigen::Quaterniond q = odom_orient_.normalized();
        // Eigen::Isometry3d T_wb(q);
        // T_wb.pretranslate(odom_pos_);
        // Eigen::Vector3d point_w = T_wb * point_b;
        // cout << "ceicle Center in world:" << point_w.transpose() << endl;
        //cout << "distance to center:" << (point_w - odom_pos_).norm() << endl;
        return point_w;
    }

至此，尽管效果不是很好，识别圆心的真实世界坐标也就结束了，接下来就可以使用这个真实圆心重新规划一下轨迹了。以一个视频作为结尾，
视频链接

1.4 主要参考资料

1 视觉SLAM14讲 高翔
2 OpenCV官网

2 轨迹重规划

我最开始想的就是如果识别到圆环真实世界坐标，就用这个和后边所有不准的waypoint再重新生成一次轨迹，一直到识别最后一个圆心为止。
一般来说这种做法是不合适的，第一，一直修改全局轨迹增加了计算负担，可能会达不到实时性。第二，即使前边修改了后边的轨迹，后边识别到环还是会在修改，所以前边修改全局轨迹是在做无用功，是一种资源浪费。所以一般的做法是跟踪全局轨迹的时候，一旦识别到圆心真实位置就局部生成一小段轨迹穿过真实圆心即可。所以我看了很多论文，学一学局部规划和全局规划怎么结合起来，高飞老师的teach and replan是滚动时域重规划，就是检测全局轨迹一段时间的碰撞，就在那一段时间重规划，ewok是几个B样条控制点范围内重规划，ETH用的多项式轨迹重规划，他们是修改全局轨迹的一段距离，反正都可以，很灵活。
但是这个比赛我就想糊弄一下就行了，因为用的是闭式解方法的minJerk轨迹生成，还没有碰撞等约束，通过几个waypoints生成的很快，那就这么整吧！
一整就出现了上面视频的一幕，识别到圆心坐标后第一段轨迹会出现一个尖儿，如下图所示，

我这个小白很是不解，因为我即将触及到规划的一个关键问题：时间分配。
轨迹的时间分配就是规定多长时间执行完这段轨迹，路径就没有这个问题了。
因为我这个全局轨迹的时间分配就是一个简单的梯形时间分配，就假设了飞机从静止开始加速到最大速度，最后速度减小到0静止，但是在线重规划时飞机起始速度不是零，这就导致第一小段分配时间太长了，所以时间分配得换一换。我也看了很多时间分配的论文，以后会试试，这次糊弄糊弄得了。
后来我就用每段的距离成一个倍数再除以最大速度得到一个不是最优的但是可以顺滑点飞行的时间分配，如下面视频所示，
视频链接
至此，这个比赛就告一段落了，草草地结束了，我这篇文章也这样草草结束了。