[OpenCV實戰]6 基于特征點比對的視訊穩像

目錄

在這篇文章中，我們将學習如何使用OpenCV庫中稱為特征點比對的技術以實作簡單視訊穩定穩像。我們将讨論該算法并共享代碼，以便在OpenCV中使用此方法設計一個簡單的穩定器，最好OpenCV3.4.3以上實作代碼。什麼是視訊穩定，視訊穩定是指用于減少相機運動對最終視訊影響的一系列方法，了解成消除視訊抖動就行了。見下圖通常用拍攝會出現輕微的抖動，比如手機拍攝視訊，後期需要對其進行視訊穩像操作。

視訊穩定的需求涉及許多領域。它在消費者和專業攝像中極為重要。是以，存在許多不同的機械，光學和算法解決方案。即使在靜态圖像拍攝中，穩定也可以幫助拍攝時間長的照片。在内窺鏡檢查和結腸鏡檢查等醫療診斷應用中，需要穩定視訊以确定問題的确切位置和寬度。類似地，在軍事應用中，飛行器在偵察飛行中捕獲的視訊需要穩定以進行定位，導航，目标跟蹤等。這同樣适用于機器人應用。

1 介紹

1.1 視訊穩定的方法

視訊穩定方法包括機械，光學和數字穩定方法。具體如下：

機械視訊穩定：機械圖像穩定系統使用陀螺儀和加速度計等特殊傳感器檢測到運動來移動圖像傳感器以補償相機的運動。

光學視訊穩定：在這種方法中，不是移動整個相機，而是通過移動鏡頭的部分來實作穩定。該方法采用可移動透鏡元件，當透過相機的透鏡系統時，可移動透鏡元件可變地調節光的路徑長度。

數字視訊穩定：此方法不需要特殊的傳感器來估算相機運動。本文就是用的這種方法。主要有三個步驟， 1）運動估計，2）運動平滑，3）圖像合成。在第一階段中導出兩個連續幀之間的變換參數。第二級濾除不需要的運動，第三個階段重建穩定的視訊。

我們将在本文中學習快速而強大的實數字視訊穩定算法。它基于二維運動模型，我們應用包含平移，旋轉和縮放的歐幾裡德（又稱相似性）變換。

如上圖所示，在歐幾裡德運動模型中，圖像中的正方形可以轉換為具有不同大小，形狀位置的任何其他四方形。它比仿射和單應變換更具限制性，但足以用于運動穩定，因為視訊的連續幀之間的相機移動通常很小。

1.2 使用點特征比對的視訊穩定

該方法涉及跟蹤兩個連續幀之間的一些特征點。跟蹤的特征允許我們估計幀之間的運動并對其進行補償。下面的流程圖顯示了算法基本步驟。

1 擷取多幀視訊圖像，擷取圖像角點(特征點)；

2 光流法跟蹤角點；根據前後兩張圖像角點變化得到表示運動的仿射變化矩陣。

3 根據仿射變化矩陣計算運動軌迹，并且平滑運動軌迹。

4 根據平滑後的運動軌迹，得到平滑運動後的仿射變化矩陣。

5 根據平滑運動後的仿射變化矩陣得到穩定後的圖像。

https://imgconvert.csdnimg.cn/aHR0cHM6Ly93d3cubGVhcm5vcGVuY3YuY29tL3dwLWNvbnRlbnQvdXBsb2Fkcy8yMDE5LzAxL0FFQU0tMy5wbmc?x-oss-process=image/format,png

2 算法

2.1 幀間運動資訊擷取

算法中最關鍵的部分是确定各幀的運動方向。我們将疊代所有幀，并找到目前幀和前一幀之間的運動。沒有必要知道每個像素的運動。歐幾裡德運動模型要求我們知道兩幀中2個點的運動資訊。然而，在實踐中最好找到50-100點的運動資訊，然後使用它們來穩健地估計運動模型

2.1.1 合适的特征點擷取

現在的問題是我們應該選擇哪些特征點進行跟蹤。請記住，跟蹤算法會使用一個以該點為圓心的圓(小孔)來近似來模拟點的運動。這種跟蹤算法受到圓直徑的影響。是以，平滑區域對于跟蹤是不利的，并且具有許多角點的紋理區域是好的。幸運的是，OpenCV具有快速檢測函數特征點的跟蹤，即函數goodFeaturesToTrack。角點個人了解是指圖像中亮度變化劇烈的點或圖像邊緣上變化交大的點。

2.1.2 Lucas-Kanade光流法

一旦我們在前一幀中找到了好的特征(角點)，我們就可以使用名為Lucas-Kanade Optical Flow的算法在下一幀中跟蹤它們，該算法以算法的發明者命名。算法詳情可以見

OpenCV中的函數calcOpticalFlowPyrLK實作算法。在calcOpticalFlowPyrLK中，LK代表Lucas-Kanade，而Pyr代表金字塔。計算機視覺中的圖像金字塔用于處理不同比例（分辨率）的圖像。但是由于各種原因，calcOpticalFlowPyrLK可能無法計算所有點的運動。例如，目前幀中的特征點可能被下一幀中的另一個對象遮擋。幸運的是calcOpticalFlowPyrLK中的狀态标志可用于過濾掉這些值。

2.1.3 運動估計

回顧一下，在步驟2.1.1中，我們發現在前一幀中要跟蹤的特征點。在步驟2.1.2中，我們使用光流來跟蹤特征點。換句話說，我們在目前幀中找到了特征點的位置，并且我們已經知道了前一幀中特征點的位置。是以，我們可以使用這兩組特征點來找到将前一幀映射到目前幀的（歐幾裡德）變換。OpenCV使用函數estimateRigidTransform完成的。仿射變化詳細見

一旦我們擷取運動資訊，我們就可以将它分解為x和y值以及平移和旋轉（角度）值。我們将這些值存儲在一個數組中，以便我們可以順利更改它們。

下面的代碼将介紹步驟2.1.1至2.1.3。請務必閱讀要遵循的代碼中的注釋。

在C ++實作中，我們首先定義一些類來幫助我們存儲估計的運動矢量。下面C++代碼的TransformParam類存儲運動資訊（dx為x軸運動資訊，dy為y軸運動資訊，da為角度資訊），并提供方法getTransform将此運動轉換為變換矩陣。

/**
 * @brief 運動資訊結構體
 *
 */
struct TransformParam
{
    TransformParam() {}
    //x軸資訊，y軸資訊，角度資訊
    TransformParam(double _dx, double _dy, double _da)
    {
        dx = _dx;
        dy = _dy;
        da = _da;
    }

    double dx;
    double dy;
    // angle
    double da;

    void getTransform(Mat &T)
    {
        // Reconstruct transformation matrix accordingly to new values 重建變換矩陣
        T.at<double>(0, 0) = cos(da);
        T.at<double>(0, 1) = -sin(da);
        T.at<double>(1, 0) = sin(da);
        T.at<double>(1, 1) = cos(da);

        T.at<double>(0, 2) = dx;
        T.at<double>(1, 2) = dy;
    }
};
           

我們循環周遊幀并執行2.1幀間運動資訊擷取所有代碼。C++代碼：

//previous transformation matrix 上一張圖像的仿射矩陣
    Mat last_T;
    //從第二幀開始循環周遊視訊所有幀
    for (int i = 1; i < n_frames; i++)
    {
        // Vector from previous and current feature points 前一幀角點vector，目前幀角點vector
        vector<Point2f> prev_pts, curr_pts;

        // Detect features in previous frame 擷取前一幀的角點
        //前一幀灰階圖，前一幀角點vector, 最大角點數，檢測到的角點的品質等級，兩個角點之間的最小距離
        goodFeaturesToTrack(prev_gray, prev_pts, 200, 0.01, 30);

        // Read next frame 讀取目前幀圖像
        bool success = cap.read(curr);
        if (!success)
        {
            break;
        }

        // Convert to grayscale 将目前幀圖像轉換為灰階圖
        cvtColor(curr, curr_gray, COLOR_BGR2GRAY);

        // Calculate optical flow (i.e. track feature points) 光流法追尋特征點
        //輸出狀态矢量(元素是無符号char類型，uchar)，如果在目前幀發現前一幀角點特征則置為1，否則，為0
        vector<uchar> status;
        //輸出誤差矢量
        vector<float> err;
        //光流跟蹤
        //前一幀灰階圖像，目前幀灰階圖像，前一幀角點，目前幀角點，狀态量，誤差量
        calcOpticalFlowPyrLK(prev_gray, curr_gray, prev_pts, curr_pts, status, err);

        // Filter only valid points 擷取光流跟蹤下有效的角點
        //周遊角點
        auto prev_it = prev_pts.begin();
        auto curr_it = curr_pts.begin();
        for (size_t k = 0; k < status.size(); k++)
        {
            if (status[k])
            {
                prev_it++;
                curr_it++;
            }
            //删除無效角點
            else
            {
                prev_it = prev_pts.erase(prev_it);
                curr_it = curr_pts.erase(curr_it);
            }
        }

        // Find transformation matrix 獲得變換矩陣
        //false表示帶幾何限制的仿射變換，true則是全仿射變化，T為變換矩陣
        Mat T = estimateRigidTransform(prev_pts, curr_pts, false);

        // In rare cases no transform is found.
        // We'll just use the last known good transform.
        //極少數情況會找不到變換矩陣，取上一個變換為目前變化矩陣
        //當然第一次檢測就沒找到仿射矩陣，算法會出問題，不過機率很低
        if (T.data == NULL)
        {
            last_T.copyTo(T);
        }
        T.copyTo(last_T);

        // Extract traslation 提取仿射變化結果
        double dx = T.at<double>(0, 2);
        double dy = T.at<double>(1, 2);

        // Extract rotation angle 提取角度
        double da = atan2(T.at<double>(1, 0), T.at<double>(0, 0));

        // Store transformation 存儲仿射變化矩陣
        transforms.push_back(TransformParam(dx, dy, da));

        // Move to next frame 進行下一次檢測準測
        curr_gray.copyTo(prev_gray);

        cout << "Frame: " << i << "/" << n_frames << " -  Tracked points : " << prev_pts.size() << endl;
    }
           

2.2 計算幀之間的總體運動

在上一步中，我們擷取了幀之間的運動情況并将它們存儲在一個數組中。我們現在需要通過累積分析幀間運動情況來找到運動軌迹。

2.2.1 計算運動軌迹

在此步驟中，我們将累加幀之間的運動以計算軌迹。我們的最終目标是平滑這一軌迹。

在Python中，使用numpy中的cumsum（累積和）很容易實作。

在C ++中，我們定義了一個名為Trajectory的類來存儲運動參數每次的累積和。

/**
 * @brief 軌迹結構體
 *
 */
struct Trajectory
{
    Trajectory() {}
    Trajectory(double _x, double _y, double _a)
    {
        x = _x;
        y = _y;
        a = _a;
    }

    double x;
    double y;
    // angle
    double a;
};
           

我們還定義了一個函數cumsum，輸入為TransformParams結構資料，并通過dx，dy和da（角度）的累積和來傳回軌迹資訊。C++代碼：

/**
 * @brief 軌迹累積
 *
 * @param transforms 運動資訊結構體
 * @return vector<Trajectory> 軌迹結構體
 */
vector<Trajectory> cumsum(vector<TransformParam> &transforms)
{
    // trajectory at all frames 所有幀的運動軌迹
    vector<Trajectory> trajectory;
    // Accumulated frame to frame transform 累加計算x,y以及a（角度）
    double a = 0;
    double x = 0;
    double y = 0;

    //累加
    for (size_t i = 0; i < transforms.size(); i++)
    {
        x += transforms[i].dx;
        y += transforms[i].dy;
        a += transforms[i].da;

        trajectory.push_back(Trajectory(x, y, a));
    }

    return trajectory;
}
           

2.2.2 計算平滑軌迹

在上一步中，我們計算了運動的軌迹。是以，我們有三條曲線顯示運動（x，y和角度）随時間的變化情況。

在這一步中，我們将展示如何平滑這三條曲線。

平滑任何曲線的最簡單方法是使用移動平均濾波器。顧名思義，移動平均濾波器将該點處的函數值替換為鄰域窗格所有點的平均值。我們來看一個例子。

比方說，我們已經存儲在數組中的曲線C，曲線上的點為C [0], … ,C [N-1]。，用窗寬為5的移動平均濾波器對曲線c濾波可以得到平滑的曲線f。計算公式如下：

如下圖，平滑曲線的值是在小視窗上平均左側噪聲曲線的值。下圖顯示了左側噪聲曲線的示例，使用右側大小為5移動平均濾波器進行平滑處理。

在C ++版本中，我們定義了一個名為smooth的函數，它計算平滑的移動平均軌迹。

/**
 * @brief 平滑運動軌迹
 *
 * @param trajectory 運動軌迹
 * @param radius 窗格大小
 * @return vector<Trajectory>
 */
vector<Trajectory> smooth(vector<Trajectory> &trajectory, int radius)
{
    //平滑後的運動軌迹
    vector<Trajectory> smoothed_trajectory;
    //移動滑動窗格
    for (size_t i = 0; i < trajectory.size(); i++)
    {
        double sum_x = 0;
        double sum_y = 0;
        double sum_a = 0;
        int count = 0;

        for (int j = -radius; j <= radius; j++)
        {
            if (i + j >= 0 && i + j < trajectory.size())
            {
                sum_x += trajectory[i + j].x;
                sum_y += trajectory[i + j].y;
                sum_a += trajectory[i + j].a;

                count++;
            }
        }

        double avg_a = sum_a / count;
        double avg_x = sum_x / count;
        double avg_y = sum_y / count;

        smoothed_trajectory.push_back(Trajectory(avg_x, avg_y, avg_a));
    }

    return smoothed_trajectory;
}
           

2.2.3 平滑變化計算

到目前為止，我們已經獲得了平滑的軌迹。在此步驟中，我們将使用平滑軌迹來獲得平滑變換，這些變換可應用于視訊幀以使其穩定。這是通過找到平滑軌迹和原始軌迹之間的差異并将該差異添加原始變換矩陣來完成的。C++代碼：

//平滑後的運動資訊結構體
    vector<TransformParam> transforms_smooth;

    //原始運動資訊結構體
    for (size_t i = 0; i < transforms.size(); i++)
    {
        // Calculate difference in smoothed_trajectory and trajectory 計算平滑後的軌迹和原始軌迹差異
        double diff_x = smoothed_trajectory[i].x - trajectory[i].x;
        double diff_y = smoothed_trajectory[i].y - trajectory[i].y;
        double diff_a = smoothed_trajectory[i].a - trajectory[i].a;

        // Calculate newer transformation array 計算平滑後的運動資訊結構體資料
        double dx = transforms[i].dx + diff_x;
        double dy = transforms[i].dy + diff_y;
        double da = transforms[i].da + diff_a;

        transforms_smooth.push_back(TransformParam(dx, dy, da));
    }
           

2.3 将平滑後的變化矩陣應用于幀

我們差不多完成了。我們現在需要做的就是周遊幀并應用我們剛剛計算的變換。

如果我們将運動指定為

x

,

y

,

θ

，則相應的變換矩陣由下式給出：

當我們穩定視訊時，我們可能會看到一些黑色邊界。這是預期的，因為要穩定視訊，視訊幀原圖像可能不得不縮小（不是圖的尺寸縮小，圖像尺寸不變。有兩種情況，一種實際先縮小圖像，然後從中截取原圖大小的區域，缺少圖像區域用黑色填充，起到圖像增大作用；另外将原圖擴大，然後截取原圖尺寸相等大小區域，起到圖像縮小作用）。我們可以通過以其視訊中點為中心縮放圖像（例如4％）來緩解該問題。下面的函數fixBorder顯示了實作。我們使用getRotationMatrix2D，因為它可以在不移動圖像中心的情況下縮放和旋轉圖像。我們需要做的就是調用此函數，旋轉0和縮放1.04(将原圖擴大為1.04倍，然後截取原圖尺寸相等大小區域)。C++代碼如下：

/**
 * @brief
 *
 * @param frame_stabilized
 */
void fixBorder(Mat &frame_stabilized)
{
    //将原圖擴大為1.04倍，然後截取原圖尺寸相等大小區域
    Mat T = getRotationMatrix2D(Point2f(frame_stabilized.cols / 2, frame_stabilized.rows / 2), 0, 1.04);
    //仿射變換
    warpAffine(frame_stabilized, frame_stabilized, T, frame_stabilized.size());
}
           

3 結果和代碼

3.1總結

這段代碼簡單來說就是計算目前幀和前一幀的仿射變換，因為視訊隻是抖動，我們可以獲得目前幀相對于前一幀的x軸變化量，y軸變化量以及角度變化量。整個視訊都這樣做一遍，把所有變換量的值儲存在了transformations這個變量。cumsum是計算累加值，意思是獲得目前幀與第一幀的偏移量，平滑一下。最後重新播放視訊的時候，我們可以進行仿射變換，根據平滑後的偏移量反向操作，使得目前幀相對于第一幀沒有偏移。

優點： - 該方法對低頻運動（較慢的振動）提供了良好的穩定性。- 該方法具有低記憶體消耗，是以非常适用于嵌入式裝置（如Raspberry Pi）。- 此方法可以很好地防止視訊中的縮放（縮放）抖動。 缺點： - 該方法對高頻擾動的影響很小。- 速度過慢。- 如果運動模糊，則功能跟蹤将失敗，結果将不是最佳。- 滾動快門失真也不适合這種方法。 ## 3.2 代碼

代碼位址：

如果沒有積分（系統自動設定資源分數）看看參考連結。我搬運過來的，大修改沒有。

代碼提供了C++和Python版本，代碼都有詳細的注釋。

// video_stabilization.cpp : 此檔案包含 "main" 函數。程式執行将在此處開始并結束。
//

#include "pch.h"
#include <opencv2/opencv.hpp>
#include <iostream>
#include <cassert>
#include <cmath>
#include <fstream>

using namespace std;
using namespace cv;

// In frames. The larger the more stable the video, but less reactive to sudden panning 移動平均滑動視窗大小
const int SMOOTHING_RADIUS = 50;

/**
 * @brief 運動資訊結構體
 *
 */
struct TransformParam
{
    TransformParam() {}
    //x軸資訊，y軸資訊，角度資訊
    TransformParam(double _dx, double _dy, double _da)
    {
        dx = _dx;
        dy = _dy;
        da = _da;
    }

    double dx;
    double dy;
    // angle
    double da;

    void getTransform(Mat &T)
    {
        // Reconstruct transformation matrix accordingly to new values 重建變換矩陣
        T.at<double>(0, 0) = cos(da);
        T.at<double>(0, 1) = -sin(da);
        T.at<double>(1, 0) = sin(da);
        T.at<double>(1, 1) = cos(da);

        T.at<double>(0, 2) = dx;
        T.at<double>(1, 2) = dy;
    }
};

/**
 * @brief 軌迹結構體
 *
 */
struct Trajectory
{
    Trajectory() {}
    Trajectory(double _x, double _y, double _a)
    {
        x = _x;
        y = _y;
        a = _a;
    }

    double x;
    double y;
    // angle
    double a;
};

/**
 * @brief 軌迹累積
 *
 * @param transforms 運動資訊結構體
 * @return vector<Trajectory> 軌迹結構體
 */
vector<Trajectory> cumsum(vector<TransformParam> &transforms)
{
    // trajectory at all frames 所有幀的運動軌迹
    vector<Trajectory> trajectory;
    // Accumulated frame to frame transform 累加計算x,y以及a（角度）
    double a = 0;
    double x = 0;
    double y = 0;

    //累加
    for (size_t i = 0; i < transforms.size(); i++)
    {
        x += transforms[i].dx;
        y += transforms[i].dy;
        a += transforms[i].da;

        trajectory.push_back(Trajectory(x, y, a));
    }

    return trajectory;
}

/**
 * @brief 平滑運動軌迹
 *
 * @param trajectory 運動軌迹
 * @param radius 窗格大小
 * @return vector<Trajectory>
 */
vector<Trajectory> smooth(vector<Trajectory> &trajectory, int radius)
{
    //平滑後的運動軌迹
    vector<Trajectory> smoothed_trajectory;
    //移動滑動窗格
    for (size_t i = 0; i < trajectory.size(); i++)
    {
        double sum_x = 0;
        double sum_y = 0;
        double sum_a = 0;
        int count = 0;

        for (int j = -radius; j <= radius; j++)
        {
            if (i + j >= 0 && i + j < trajectory.size())
            {
                sum_x += trajectory[i + j].x;
                sum_y += trajectory[i + j].y;
                sum_a += trajectory[i + j].a;

                count++;
            }
        }

        double avg_a = sum_a / count;
        double avg_x = sum_x / count;
        double avg_y = sum_y / count;

        smoothed_trajectory.push_back(Trajectory(avg_x, avg_y, avg_a));
    }

    return smoothed_trajectory;
}

/**
 * @brief
 *
 * @param frame_stabilized
 */
void fixBorder(Mat &frame_stabilized)
{
    //将原圖擴大為1.04倍，然後截取原圖尺寸相等大小區域
    Mat T = getRotationMatrix2D(Point2f(frame_stabilized.cols / 2, frame_stabilized.rows / 2), 0, 1.04);
    //仿射變換
    warpAffine(frame_stabilized, frame_stabilized, T, frame_stabilized.size());
}

int main(int argc, char **argv)
{
    // Read input video 讀取視訊
    VideoCapture cap("./video/detect.mp4");

    // Get frame count 讀取視訊總幀數
    int n_frames = int(cap.get(CAP_PROP_FRAME_COUNT));
    // Our test video may be wrong to read the frame after frame 1300
    n_frames = 1300;

    // Get width and height of video stream 擷取視訊圖像寬高
    int w = int(cap.get(CAP_PROP_FRAME_WIDTH));
    int h = int(cap.get(CAP_PROP_FRAME_HEIGHT));

    // Get frames per second (fps) 擷取視訊每秒幀數
    double fps = cap.get(CV_CAP_PROP_FPS);

    // Set up output video 設定輸出視訊
    VideoWriter out("video_out.avi", CV_FOURCC('M', 'J', 'P', 'G'), fps, Size(2 * w, h));

    // Define variable for storing frames 定義存儲幀的相關變量
    //目前幀RGB圖像和灰階圖
    Mat curr, curr_gray;
    //前一幀RGB圖像和灰階圖
    Mat prev, prev_gray;

    // Read first frame 獲得視訊一張圖象
    cap >> prev;

    // Convert frame to grayscale 轉換為灰階圖
    cvtColor(prev, prev_gray, COLOR_BGR2GRAY);

    // Pre-define transformation-store array 仿射變化參數結構體
    vector<TransformParam> transforms;

    //previous transformation matrix 上一張圖像的仿射矩陣
    Mat last_T;
    //從第二幀開始循環周遊視訊所有幀
    for (int i = 1; i < n_frames; i++)
    {
        // Vector from previous and current feature points 前一幀角點vector，目前幀角點vector
        vector<Point2f> prev_pts, curr_pts;

        // Detect features in previous frame 擷取前一幀的角點
        //前一幀灰階圖，前一幀角點vector, 最大角點數，檢測到的角點的品質等級，兩個角點之間的最小距離
        goodFeaturesToTrack(prev_gray, prev_pts, 200, 0.01, 30);

        // Read next frame 讀取目前幀圖像
        bool success = cap.read(curr);
        if (!success)
        {
            break;
        }

        // Convert to grayscale 将目前幀圖像轉換為灰階圖
        cvtColor(curr, curr_gray, COLOR_BGR2GRAY);

        // Calculate optical flow (i.e. track feature points) 光流法追尋特征點
        //輸出狀态矢量(元素是無符号char類型，uchar)，如果在目前幀發現前一幀角點特征則置為1，否則，為0
        vector<uchar> status;
        //輸出誤差矢量
        vector<float> err;
        //光流跟蹤
        //前一幀灰階圖像，目前幀灰階圖像，前一幀角點，目前幀角點，狀态量，誤差量
        calcOpticalFlowPyrLK(prev_gray, curr_gray, prev_pts, curr_pts, status, err);

        // Filter only valid points 擷取光流跟蹤下有效的角點
        //周遊角點
        auto prev_it = prev_pts.begin();
        auto curr_it = curr_pts.begin();
        for (size_t k = 0; k < status.size(); k++)
        {
            if (status[k])
            {
                prev_it++;
                curr_it++;
            }
            //删除無效角點
            else
            {
                prev_it = prev_pts.erase(prev_it);
                curr_it = curr_pts.erase(curr_it);
            }
        }

        // Find transformation matrix 獲得變換矩陣
        //false表示帶幾何限制的仿射變換，true則是全仿射變化，T為變換矩陣
        Mat T = estimateRigidTransform(prev_pts, curr_pts, false);

        // In rare cases no transform is found.
        // We'll just use the last known good transform.
        //極少數情況會找不到變換矩陣，取上一個變換為目前變化矩陣
        //當然第一次檢測就沒找到仿射矩陣，算法會出問題，不過機率很低
        if (T.data == NULL)
        {
            last_T.copyTo(T);
        }
        T.copyTo(last_T);

        // Extract traslation 提取仿射變化結果
        double dx = T.at<double>(0, 2);
        double dy = T.at<double>(1, 2);

        // Extract rotation angle 提取角度
        double da = atan2(T.at<double>(1, 0), T.at<double>(0, 0));

        // Store transformation 存儲仿射變化矩陣
        transforms.push_back(TransformParam(dx, dy, da));

        // Move to next frame 進行下一次檢測準測
        curr_gray.copyTo(prev_gray);

        cout << "Frame: " << i << "/" << n_frames << " -  Tracked points : " << prev_pts.size() << endl;
    }

    // Compute trajectory using cumulative sum of transformations 擷取累加軌迹
    vector<Trajectory> trajectory = cumsum(transforms);

    // Smooth trajectory using moving average filter 擷取平滑後的軌迹
    vector<Trajectory> smoothed_trajectory = smooth(trajectory, SMOOTHING_RADIUS);

    //平滑後的運動資訊結構體
    vector<TransformParam> transforms_smooth;

    //原始運動資訊結構體
    for (size_t i = 0; i < transforms.size(); i++)
    {
        // Calculate difference in smoothed_trajectory and trajectory 計算平滑後的軌迹和原始軌迹差異
        double diff_x = smoothed_trajectory[i].x - trajectory[i].x;
        double diff_y = smoothed_trajectory[i].y - trajectory[i].y;
        double diff_a = smoothed_trajectory[i].a - trajectory[i].a;

        // Calculate newer transformation array 計算平滑後的運動資訊結構體資料
        double dx = transforms[i].dx + diff_x;
        double dy = transforms[i].dy + diff_y;
        double da = transforms[i].da + diff_a;

        transforms_smooth.push_back(TransformParam(dx, dy, da));
    }

    //定位目前幀為第1幀
    cap.set(CV_CAP_PROP_POS_FRAMES, 0);
    //平滑後的變化矩陣
    Mat T(2, 3, CV_64F);
    Mat frame, frame_stabilized, frame_out;

    //對所有幀進行變化得到穩像結果
    //跳過第一幀
    cap.read(frame);
    for (int i = 0; i < n_frames - 1; i++)
    {
        bool success = cap.read(frame);
        if (!success)
        {
            break;
        }
        // Extract transform from translation and rotation angle. 提取平滑後的仿射變化矩陣
        transforms_smooth[i].getTransform(T);

        // Apply affine wrapping to the given frame 應用仿射變化
        warpAffine(frame, frame_stabilized, T, frame.size());

        // Scale image to remove black border artifact 去除黑邊
        fixBorder(frame_stabilized);

        // Now draw the original and stablised side by side for coolness 将原圖和變化後的圖橫向排列輸出到視訊
        hconcat(frame, frame_stabilized, frame_out);

        // If the image is too big, resize it.
        if (frame_out.cols > 1920)
        {
            resize(frame_out, frame_out, Size(frame_out.cols / 2, frame_out.rows / 2));
        }

        //imshow("Before and After", frame_out);
        out.write(frame_out);
        cout << "out frame：" << i << endl;
        //waitKey(10);
    }

    // Release video
    cap.release();
    out.release();
    // Close windows
    destroyAllWindows();

    return 0;
}
           

# Import numpy and OpenCV
import numpy as np
import cv2

def movingAverage(curve, radius):
    window_size = 2 * radius + 1
    # Define the filter
    f = np.ones(window_size)/window_size
    # Add padding to the boundaries
    curve_pad = np.lib.pad(curve, (radius, radius), 'edge')
    # Apply convolution
    curve_smoothed = np.convolve(curve_pad, f, mode='same')
    # Remove padding
    curve_smoothed = curve_smoothed[radius:-radius]
    # return smoothed curve
    return curve_smoothed

def smooth(trajectory):
    smoothed_trajectory = np.copy(trajectory)
    # Filter the x, y and angle curves
    for i in range(3):
        smoothed_trajectory[:, i] = movingAverage(
            trajectory[:, i], radius=SMOOTHING_RADIUS)

    return smoothed_trajectory

def fixBorder(frame):
    s = frame.shape
    # Scale the image 4% without moving the center
    T = cv2.getRotationMatrix2D((s[1]/2, s[0]/2), 0, 1.04)
    frame = cv2.warpAffine(frame, T, (s[1], s[0]))
    return frame

# The larger the more stable the video, but less reactive to sudden panning
SMOOTHING_RADIUS = 50

# Read input video
cap = cv2.VideoCapture('video/detect.mp4')

# Get frame count
n_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))

# Our test video may be wrong to read the frame after frame 1300
n_frames = 1300

# Get width and height of video stream
w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))

# Get frames per second (fps)
fps = cap.get(cv2.CAP_PROP_FPS)

# Define the codec for output video
fourcc = cv2.VideoWriter_fourcc(*'MJPG')

# Set up output video
out = cv2.VideoWriter('video_out.avi', fourcc, fps, (2 * w, h))

# Read first frame
_, prev = cap.read()

# Convert frame to grayscale
prev_gray = cv2.cvtColor(prev, cv2.COLOR_BGR2GRAY)

# Pre-define transformation-store array
transforms = np.zeros((n_frames-1, 3), np.float32)

for i in range(n_frames-2):
    # Detect feature points in previous frame
    prev_pts = cv2.goodFeaturesToTrack(prev_gray,
                                       maxCorners=200,
                                       qualityLevel=0.01,
                                       minDistance=30,
                                       blockSize=3)

    # Read next frame
    success, curr = cap.read()
    if not success:
        break

    # Convert to grayscale
    curr_gray = cv2.cvtColor(curr, cv2.COLOR_BGR2GRAY)

    # Calculate optical flow (i.e. track feature points)
    curr_pts, status, err = cv2.calcOpticalFlowPyrLK(
        prev_gray, curr_gray, prev_pts, None)

    # Sanity check
    assert prev_pts.shape == curr_pts.shape

    # Filter only valid points
    idx = np.where(status == 1)[0]
    prev_pts = prev_pts[idx]
    curr_pts = curr_pts[idx]

    # Find transformation matrix
    # will only work with OpenCV-3 or less
    m = cv2.estimateRigidTransform(prev_pts, curr_pts, fullAffine=False)

    # Extract traslation
    dx = m[0, 2]
    dy = m[1, 2]

    # Extract rotation angle
    da = np.arctan2(m[1, 0], m[0, 0])

    # Store transformation
    transforms[i] = [dx, dy, da]

    # Move to next frame
    prev_gray = curr_gray

    print("Frame: " + str(i) + "/" + str(n_frames) +
          " -  Tracked points : " + str(len(prev_pts)))

# Compute trajectory using cumulative sum of transformations
trajectory = np.cumsum(transforms, axis=0)

# Create variable to store smoothed trajectory
smoothed_trajectory = smooth(trajectory)

# Calculate difference in smoothed_trajectory and trajectory
difference = smoothed_trajectory - trajectory

# Calculate newer transformation array
transforms_smooth = transforms + difference

# Reset stream to first frame
cap.set(cv2.CAP_PROP_POS_FRAMES, 0)

# Write n_frames-1 transformed frames
for i in range(n_frames-2):
    # Read next frame
    success, frame = cap.read()
    if not success:
        break

    # Extract transformations from the new transformation array
    dx = transforms_smooth[i, 0]
    dy = transforms_smooth[i, 1]
    da = transforms_smooth[i, 2]

    # Reconstruct transformation matrix accordingly to new values
    m = np.zeros((2, 3), np.float32)
    m[0, 0] = np.cos(da)
    m[0, 1] = -np.sin(da)
    m[1, 0] = np.sin(da)
    m[1, 1] = np.cos(da)
    m[0, 2] = dx
    m[1, 2] = dy

    # Apply affine wrapping to the given frame
    frame_stabilized = cv2.warpAffine(frame, m, (w, h))

    # Fix border artifacts
    frame_stabilized = fixBorder(frame_stabilized)

    # Write the frame to the file
    frame_out = cv2.hconcat([frame, frame_stabilized])

    # If the image is too big, resize it.
    if(frame_out.shape[1] > 1920):
        frame_out = cv2.resize(
            frame_out, (frame_out.shape[1]/2, frame_out.shape[0]/2))

    #cv2.imshow("Before and After", frame_out)
    # cv2.waitKey(10)
    out.write(frame_out)

# Release video
cap.release()
out.release()
# Close windows
cv2.destroyAllWindows()