-
仿射变换（平移、翻转、旋转、缩放、错切五种变换的组合）
-
RANSAC
RANSAC是"RANdom SAmple Consensus"（随机采样一致）的缩写。该算法的主要功能是通过一组包含“局外点”（即错误点）通过迭代的方式估计数学模型的参数。它是一种不确定的算法——有一定的概率得出一个合理的结果。
1）RANSAC的假设
- 数据由“局内点”组成，数据的分布可以用一些模型参数来解释。
- 数据的“局外点”，是不能适应该模型的数据。
- 给定一组局内点（数量相对于整体数据少），存在一个可以估计模型参数的过程，该模型能够解释或使用局内点。
- 局外点的产生原因有：噪声，错误的观测，对数据的错误假设等。
2）基本思想
RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：
- 有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
- 用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
- 如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
- 然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
- 最后，通过估计局内点与模型的错误率来评估模型。
- 这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被舍弃，要么因为比现有的模型更好而被选用。
3）RANSAC拟合直线
#include<iostream>
#include<opencv2/opencv.hpp>
#include<stdlib.h>
using namespace std;
using namespace cv;
//生成[0,1]之间符合均匀分布的数
double uniformRandom(void)
{
return (double)rand() / (double)RAND_MAX;
}
//生成【0，1】之间的符合高斯分布的随机数
double gaussianRandom(void)
{
static int next_gaussian = 0;
static double saved_gaussian_value;
double fac,rsq, v1, v2;
if (next_gaussian == 0) {
do {
v1 = 2 * uniformRandom() - 1;
v2 = 2 * uniformRandom() - 1;
rsq = v1*v1 + v2*v2;
} while (rsq >= 1.0 || rsq == 0.0);
fac = sqrt(-2 * log(rsq) / rsq);
saved_gaussian_value = v1*fac;
next_gaussian = 1;
return v2*fac;
}
else {
next_gaussian = 0;
return saved_gaussian_value;
}
}
void get_inline_outline(vector<Point2d> &ptSet)
{
//图像大小
int width = 640;
int height = 320;
//直线参数
double a = 1, b = 2, c = -640;
//随机获取直线上20个点
srand((unsigned int)time(NULL)); //设置随机数种子
while (true)
{
double x = uniformRandom()*(width - 1);
double y = -(a*x + c) / b;
//加0.5高斯噪声
x += gaussianRandom()*0.5;
y += gaussianRandom()*0.5;
if (x >= 640 && y >= 320)
continue;
Point2d pt(x, y);
ptSet.push_back(pt);
if (ptSet.size() == 20)
break;
}
//随机获取10个野值点
while (true)
{
double x = uniformRandom() * (width - 1);
double y = uniformRandom() * (height - 1);
if (fabs(a*x + b*y + c) < 10) //野值点到直线距离不小于10个像素
continue;
Point2d pt(x, y);
ptSet.push_back(pt);
if (ptSet.size() == 30)
break;
}
}
//根据点集拟合直线ax+by+c=0
void calcLinePara(vector<Point2d> pts, double &a, double &b, double &c)
{
Vec4f line;
fitLine(pts, line, CV_DIST_L2, 0, 1e-2, 1e-2);
a = line[1];
b = -line[0];
c = line[0] * line[3] - line[1] * line[2];
}
//得到直线拟合样本，即在直线采样点集上随机选2个点
bool getSample(vector<int> set, vector<int> &sset)
{
int i[2];
if (set.size() > 2)
{
do
{
for (int n = 0; n < 2; n++)
i[n] = int(uniformRandom() * (set.size() - 1));
} while (!(i[1] != i[0]));
for (int n = 0; n < 2; n++)
{
sset.push_back(i[n]);
}
}
else
{
return false;
}
return true;
}
//RANSAC直线拟合
void fitLineRANSAC(vector<Point2d> ptSet, double &a, double &b, double &c, vector<bool> &inlierFlag)
{
double residual_error = 2.99; //内点阈值
bool stop_loop = false;
int maximum = 0; //最大内点数
//最终内点标识及其残差
inlierFlag = vector<bool>(ptSet.size(), false);
vector<double> resids_(ptSet.size(), 3);
int sample_count = 0;
int N = 500;
// RANSAC
srand((unsigned int)time(NULL)); //设置随机数种子
vector<int> ptsID;
for (unsigned int i = 0; i < ptSet.size(); i++)
ptsID.push_back(i);
while (N > sample_count && !stop_loop)
{
vector<bool> inlierstemp;
vector<double> residualstemp;
vector<int> ptss;
int inlier_count = 0;
if (!getSample(ptsID, ptss))
{
stop_loop = true;
continue;
}
vector<Point2d> pt_sam;
pt_sam.push_back(ptSet[ptss[0]]);
pt_sam.push_back(ptSet[ptss[1]]);
if (abs(pt_sam[0].x - pt_sam[1].x) < 5 && abs(pt_sam[0].y - pt_sam[1].y) < 5)
{
++sample_count;
continue;
}
// 计算直线方程
calcLinePara(pt_sam, a, b, c);
//内点检验
for (unsigned int i = 0; i < ptSet.size(); i++)
{
Point2d pt = ptSet[i];
double resid_ = fabs(pt.x * a + pt.y * b + c);
residualstemp.push_back(resid_);
inlierstemp.push_back(false);
if (resid_ < residual_error)
{
++inlier_count;
inlierstemp[i] = true;
}
}
// 找到最佳拟合直线
if (inlier_count >= maximum)
{
maximum = inlier_count;
resids_ = residualstemp;
inlierFlag = inlierstemp;
}
// 更新RANSAC迭代次数，以及内点概率
if (inlier_count == 0)
{
N = 500;
}
else
{
double epsilon = 1.0 - double(inlier_count) / (double)ptSet.size(); //野值点比例
double p = 0.99; //所有样本中存在1个好样本的概率
double s = 2.0;
N = int(log(1.0 - p) / log(1.0 - pow((1.0 - epsilon), s)));
}
++sample_count;
}
//利用所有内点重新拟合直线
vector<Point2d> pset;
for (unsigned int i = 0; i < ptSet.size(); i++)
{
if (inlierFlag[i])
pset.push_back(ptSet[i]);
}
calcLinePara(pset, a, b, c);
}
int main()
{
vector<Point2d> ptSet;
get_inline_outline(ptSet);
//绘制点集中所有点
Mat img(321, 641, CV_8UC3, Scalar(255, 255, 255));
double A, B, C;
vector<bool> inliers;
fitLineRANSAC(ptSet, A, B, C, inliers);
for (unsigned int i = 0; i < ptSet.size(); i++) {
if (inliers[i])
circle(img, ptSet[i], 3, Scalar(0, 255, 0), 3, 16);
else
circle(img, ptSet[i], 3, Scalar(0, 0, 255), 3, 16);
}
B = B / A;
C = C / A;
A = A / A;
//绘制直线
Point2d ptStart, ptEnd;
ptStart.x = 0;
ptStart.y = -(A*ptStart.x + C) / B;
ptEnd.x = -(B*ptEnd.y + C) / A;
ptEnd.y = 0;
line(img, ptStart, ptEnd, Scalar(0, 0, 0), 1, 16);
cout << "A:" << A << " " << "B:" << B << " " << "C:" << C << " " << endl;
imshow("line fitting", img);
waitKey();
}
3）RANSAC的优缺点：
- 优点：鲁棒估计模型参数，从包含大量的外点的数据集中估计出高精度参数
- 缺点：随机估计，并设置迭代上限，有可能得不到最优结果甚至是错误结果。只能从特定的数据模型中估计一个模型，如果存在两个或多个模型，则无法估计别的模型。
４）放射变换ransac实现
#include <iostream>
#include<opencv2/xfeatures2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include<opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>
#include <mynteyed/camera.h>
#include <mynteyed/utils.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <time.h>
#include <cstdlib>
MYNTEYE_USE_NAMESPACE
cv::Mat depth;
int Num=500;
using namespace std;
class AffineRansac
{
public:
AffineRansac(int N,double Epsilon):max_iterations(N),threshold(Epsilon)
{
srand((unsigned int)time(NULL));
}
bool AffineTransformer(std::vector<cv::Point2d> Image_Coords_1, std::vector<cv::Point2d> Image_Coords_2,Eigen::Matrix<double,3,3> &A)
{
if(Image_Coords_1.size()<3||Image_Coords_2.size()<3)
{
std::cout<<"Incorrect input dimensions"<<std::endl;
return false;
}
else if(Image_Coords_1.size()!=Image_Coords_2.size())
{
std::cout<<"Mismatch between number of points in the image pair"<<std::endl;
return false;
}
Eigen::Matrix<double,Eigen::Dynamic,3> P;
Eigen::Matrix<double,Eigen::Dynamic,3> Q;
P.resize(Image_Coords_1.size(),3);
Q.resize(Image_Coords_1.size(),3);
for(int i=0;i<Image_Coords_1.size();i++)
{
P(i,0)=Image_Coords_1[i].x;
P(i,1)=Image_Coords_1[i].y;
P(i,2)=1;
Q(i,0)=Image_Coords_2[i].x;
Q(i,1)=Image_Coords_2[i].y;
Q(i,2)=1;
}
Eigen::Matrix<double,3,3> P_inv;
Eigen::Matrix<double,3,Eigen::Dynamic> P_;
P_inv=P.transpose()*P;
P_=(P_inv.inverse())*(P.transpose());
A =P_*Q;
return true;
}
double SumEucDistance(cv::Point2d const &P1, cv::Point2d const &P1_Co,
cv::Point2d const &P2, cv::Point2d const &P2_Co)
{
double d=std::sqrt(pow((P1.x-P1_Co.x),2)+pow((P1.y-P1_Co.y),2))+std::sqrt(pow((P2.x-P2_Co.x),2)+pow((P2.y-P2_Co.y),2));
return d;
}
void find_inliers (std::vector<cv::Point2d> Coordinates_Image_1, std::vector<cv::Point2d> Coordinates_Image_2,Eigen::Matrix<double,3,3> const &A, std::vector<int> &result)
{
result.resize(0);
Eigen::Matrix<double,1,3> X2;
Eigen::Matrix<double,1,3> X1;
Eigen::Matrix<double,1,3> x2;
Eigen::Matrix<double,1,3> x1;
cv::Point2d P1,P2,P1_Co,P2_Co;
double ssd;
for(int i=0;i<Coordinates_Image_1.size();i++)
{
x1(0,0)=Coordinates_Image_1[i].x;
x1(0,1)=Coordinates_Image_1[i].y;
x1(0,2)=1;
x2(0,0)=Coordinates_Image_2[i].x;
x2(0,1)=Coordinates_Image_2[i].y;
x2(0,2)=1;
X2=A*x1.transpose();
X1=A.inverse()*x2.transpose();
P1.x=X1(0,0);
P1.y=X1(0,1);
P2.x=X2(0,0);
P2.y=X2(0,1);
P1_Co.x=x1(0,0);
P1_Co.y=x1(0,1);
P2_Co.x=x2(0,0);
P2_Co.y=x2(0,1);
//std::cout<<"******X1: "<<P1<<std::endl;
//std::cout<<"******X2: "<<P2<<std::endl;
//std::cout<<"******x1: "<<P1_Co<<std::endl;
//std::cout<<"******x2: "<<P2_Co<<std::endl;
ssd=SumEucDistance(P1,P1_Co,P2,P2_Co);
//std::cout<<"ssd: "<<ssd<<std::endl;
if(ssd<threshold)
{
result.push_back(i);
}
}
//std::cout<<"*************"<<std::endl;
}
void Affine_Ransac(std::vector<cv::Point2d> Coordinates_Image_1, std::vector<cv::Point2d> Coordinates_Image_2)
{
int In_count = 0;
std::vector<int> inliers;
inliers.reserve(Coordinates_Image_1.size());
std::vector<cv::Point2d> Image_Coords_1;
std::vector<cv::Point2d> Image_Coords_2;
Image_Coords_1.reserve(Coordinates_Image_1.size()/2+1);
Image_Coords_2.reserve(Coordinates_Image_1.size()/2+1);
std::cout<<"max_iterations: "<<max_iterations<<std::endl;
while (In_count < max_iterations)
{
// Randomly pick out points from the matches
std::set<int> subsample;
generateRandomArray(subsample,Coordinates_Image_1.size()/2+1,Coordinates_Image_1.size());
for(std::set<int>::iterator it=subsample.begin();it!=subsample.end();it++)
{
//std::cout<<"set: "<<*it<<" ";
Image_Coords_1.push_back(Coordinates_Image_1[*it]);
//std::cout<<"p1: "<<Coordinates_Image_1[*it].x<<" "<<Coordinates_Image_1[*it].y<<" ";
Image_Coords_2.push_back(Coordinates_Image_2[*it]);
//std::cout<<"p2: "<<Coordinates_Image_2[*it].x<<" "<<Coordinates_Image_2[*it].y<<" ";
}
subsample.clear();
//std::cout<<"size: "<<subsample.size()<<std::endl;
//affine
Eigen::Matrix<double,3,3> A;
bool flag=AffineTransformer(Image_Coords_1, Image_Coords_2,A);
//std::cout<<"A: "<<A<<std::endl;
if(!flag)
continue;
find_inliers(Coordinates_Image_1,Coordinates_Image_2,A,inliers);
//std::cout<<"size: "<<inliers.size()<<std::endl;
if (inliers.size() > result_inliers.size())
{
std::cout<<"counter: "<<In_count<<std::endl;
std::cout<<"size1: "<<inliers.size()<<std::endl;
std::cout<<"size2: "<<Coordinates_Image_1.size()<<std::endl;
AffineMatrix = A;
std::swap(result_inliers, inliers);
inliers.reserve(Coordinates_Image_1.size());
}
In_count++;
//std::cout<<"counter: "<<In_count<<std::endl;
}
}
private:
double uniformRandom(void)
{
return (double)std::rand()/(double)RAND_MAX;
}
// 创建一个 n个元素的数组
void generateRandomArray(std::set<int> &arr, unsigned int n, int size)
{
// 随机种子
while(arr.size()<n)
arr.insert(int(uniformRandom()*(size-1)));
}
int max_iterations;
/**
* Threshold used to determine inliers. Defaults to 0.0015.
* This threshold assumes that the input points are normalized.
*/
double threshold;
/**
* The resulting AffineMatrix which led to the inliers.
* This is NOT the re-computed matrix from the inliers.
*/
Eigen::Matrix<double,3,3> AffineMatrix;
/**
* The indices of inliers in the correspondences which led to the
* homography matrix.
*/
std::vector<int> result_inliers;
};
cv::Point2f pixel2cam ( const cv::Point2d& p, const cv::Mat& K )
{
return cv::Point2f
(
( p.x - K.at<double>(0,2) ) / K.at<double>(0,0),
( p.y - K.at<double>(1,2) ) / K.at<double>(1,1)
);
}
int main(int argc, char const *argv[])
{
//左右视图的特征点
std::vector<cv::KeyPoint> keypoints_l,keypoints_r;
cv::Mat descriptors_l,descriptors_r;
//存储左图、右图的特征点
std::vector<cv::KeyPoint> pre_keypoints_l,pre_keypoints_r;
//存储左图和右图的描述子
cv::Mat pre_descriptors_l,pre_descriptors_r;
//前一帧和当前帧
cv::Mat previous_frame_l,previous_frame_r,current_frame_l,current_frame_r;
std::vector<cv::Point2d> Coordinates_Image_1, Coordinates_Image_2;
//相机
Camera cam;
//设备
DeviceInfo dev_info;
//选择相机
if (!util::select(cam, &dev_info)) {
return 1;
}
util::print_stream_infos(cam, dev_info.index);
std::cout << "Open device: " << dev_info.index << ", "
<< dev_info.name << std::endl << std::endl;
//参数设置
OpenParams params(dev_info.index);//构造函数显式调用成功
params.color_mode=ColorMode ::COLOR_RECTIFIED;
params.depth_mode = DepthMode::DEPTH_COLORFUL;
params.stream_mode = StreamMode::STREAM_1280x480;
params.ir_intensity = 10;
params.framerate = 60;
cam.Open(params);
std::cout << std::endl;
if (!cam.IsOpened()) {
std::cerr << "Error: Open camera failed" << std::endl;
return 1;
}
std::cout << "Open device success" << std::endl << std::endl;
std::cout << "Press ESC/Q on Windows to terminate" << std::endl;
cv::namedWindow("left");
cv::namedWindow("right");
cv::namedWindow("depth");
cv::Ptr<cv::xfeatures2d::SIFT> f2d=cv::xfeatures2d::SIFT::create(2000);
//获取第一帧图像
auto right_color = cam.GetStreamData(ImageType::IMAGE_RIGHT_COLOR);
auto left_color = cam.GetStreamData(ImageType::IMAGE_LEFT_COLOR);
if (right_color.img && left_color.img)
{
auto left_img = left_color.img->To(ImageFormat::COLOR_BGR);
previous_frame_l = cv::Mat(left_img->height(), left_img->width(), CV_8UC3,
left_img->data());
auto right_img = right_color.img->To(ImageFormat::COLOR_BGR);
previous_frame_r = cv::Mat(right_img->height(), right_img->width(), CV_8UC3,
right_img->data());
//检测并计算前一帧的特征点和描述子
f2d->detectAndCompute(previous_frame_l,cv::Mat(),keypoints_l,descriptors_l);
f2d->detectAndCompute(previous_frame_r,cv::Mat(),keypoints_r,descriptors_r);
//存储左图和右图的特征点
pre_keypoints_l.insert(pre_keypoints_l.end(),keypoints_l.begin(),keypoints_l.end());
pre_keypoints_r.insert(pre_keypoints_r.end(),keypoints_r.begin(),keypoints_r.end());
//存储左图的描述子
descriptors_l.copyTo(pre_descriptors_l);
}
while(true)
{
right_color = cam.GetStreamData(ImageType::IMAGE_RIGHT_COLOR);
left_color = cam.GetStreamData(ImageType::IMAGE_LEFT_COLOR);
if (right_color.img && left_color.img)
{
auto left_img = left_color.img->To(ImageFormat::COLOR_BGR);
current_frame_l=cv::Mat(left_img->height(), left_img->width(), CV_8UC3,
left_img->data());
//获取当前帧图像
auto right_img = right_color.img->To(ImageFormat::COLOR_BGR);
current_frame_r=cv::Mat(right_img->height(), right_img->width(), CV_8UC3,
right_img->data());
//显示左图和右图
cv::imshow("cright", current_frame_r);
cv::imshow("cleft", current_frame_l);
//检测并计算当前帧的特征点和描述子
f2d->detectAndCompute(current_frame_r,cv::Mat(),keypoints_r,descriptors_r);
f2d->detectAndCompute(current_frame_l,cv::Mat(),keypoints_l,descriptors_l);
cv::Ptr<cv::DescriptorMatcher> matcher=cv::DescriptorMatcher::create(cv::DescriptorMatcher::FLANNBASED);
std::vector<std::vector<cv::DMatch>> knn_matches;
matcher->knnMatch(pre_descriptors_l,descriptors_l,knn_matches,2);
const float ratio_thresh=0.8f;
std::vector<cv::DMatch> good_matches;
for(int i=0;i<knn_matches.size();i++)
{
if((knn_matches[i][0].distance<ratio_thresh*knn_matches[i][1].distance))
good_matches.push_back(knn_matches[i][0]);
}
cv::Mat img_matches;
cv::drawMatches(previous_frame_l,pre_keypoints_l,current_frame_l,keypoints_l,good_matches,img_matches);
cv::imshow("match",img_matches);
for(int i=0;i<good_matches.size();i++)
{
Coordinates_Image_1.push_back(pre_keypoints_l[good_matches[i].queryIdx].pt);
//std::cout<<"size: "<<Coordinates_Image_1.size()<<" x1: "<<pre_keypoints_l[good_matches[i].queryIdx].pt.x<<" y1: "<<pre_keypoints_l[good_matches[i].queryIdx].pt.y<<std::endl;
Coordinates_Image_2.push_back(keypoints_l[good_matches[i].trainIdx].pt);
//std::cout<<"size: "<<Coordinates_Image_1.size()<<" x2: "<<keypoints_l[good_matches[i].queryIdx].pt.x<<" y2: "<<keypoints_l[good_matches[i].queryIdx].pt.y<<std::endl;
}
AffineRansac affine(100,1.5);
Eigen::Matrix<double,3,3> A;
affine.Affine_Ransac(Coordinates_Image_1,Coordinates_Image_2);
Coordinates_Image_1.clear();
Coordinates_Image_2.clear();
cv::swap(previous_frame_l,current_frame_l);
cv::swap(pre_descriptors_l,descriptors_l);
std::swap(pre_keypoints_l,keypoints_l);
char key = static_cast<char>(cv::waitKey(1));
if (key == 27 || key == 'q' || key == 'Q') { // ESC/Q
break;
}
}
auto image_depth = cam.GetStreamData(ImageType::IMAGE_DEPTH);
if (image_depth.img) {
auto depth_img = image_depth.img->To(ImageFormat::COLOR_BGR);
depth=cv::Mat(depth_img->height(), depth_img->width(), CV_8UC3,
depth_img->data());
cv::imshow("depth", depth);
}
}
cam.Close();
cv::destroyAllWindows();
return 0;
}
参考文章：
https://www.bbsmax.com/A/KE5QEOgy5L/
