Imu_heading源码阅读（三）——Ransac

Ransac_fitline：这部分主要是Ransac（随即样本采样一致性）算法的具体实现，首先剖析Ransac的基本原理。

RANSAC的基本假设是：
（1）数据由“局内点”组成，例如：数据的分布可以用一些模型参数来解释；
（2）“局外点”是不能适应该模型的数据；
（3）除此之外的数据属于噪声。
局外点产生的原因有：噪声的极值；错误的测量方法；对数据的错误假设。
RANSAC也做了以下假设：给定一组（通常很小的）局内点，存在一个可以估计模型参数的过程；而该模型能够解释或者适用于局内点。

一、示例
一个简单的例子是从一组观测数据中找出合适的2维直线。假设观测数据中包含局内点和局外点，其中局内点近似的被直线所通过，而局外点远离于直线。简单的最小二乘法不能找到适应于局内点的直线，原因是最小二乘法尽量去适应包括局外点在内的所有点。相反，RANSAC能得出一个仅仅用局内点计算出模型，并且概率还足够高。但是，RANSAC并不能保证结果一定正确，为了保证算法有足够高的合理概率，我们必须小心的选择算法的参数。

二、概述
RANSAC算法的输入是一组观测数据，一个可以解释或者适应于观测数据的参数化模型，一些可信的参数。
RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：
1.有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
2.用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
3.如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
4.然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
5.最后，通过估计局内点与模型的错误率来评估模型。
这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被舍弃，要么因为比现有的模型更好而被选用。

三、算法

伪码形式的算法如下所示：
输入：
data —— 一组观测数据
model —— 适应于数据的模型
n —— 适用于模型的最少数据个数
k —— 算法的迭代次数
t —— 用于决定数据是否适应于模型的阀值
d —— 判定模型是否适用于数据集的数据数目
输出：
best_model —— 跟数据最匹配的模型参数（如果没有找到好的模型，返回null）
best_consensus_set —— 估计出模型的数据点
best_error —— 跟数据相关的估计出的模型错误

iterations = 0
best_model = null
best_consensus_set = null
best_error = 无穷大
while ( iterations < k )
maybe_inliers = 从数据集中随机选择n个点
maybe_model = 适合于maybe_inliers的模型参数
consensus_set = maybe_inliersfor ( 每个数据集中不属于maybe_inliers的点 ）
if ( 如果点适合于maybe_model，且错误小于t ）
将点添加到consensus_set
if （ consensus_set中的元素数目大于d ）
已经找到了好的模型，现在测试该模型到底有多好
better_model = 适合于添加点后的consensus_set中所有点的模型参数
this_error = better_model究竟如何适合这些点的度量
if ( this_error < best_error )
我们发现了比以前好的模型，保存该模型直到更好的模型出现
best_model =  better_model
best_consensus_set = consensus_set
best_error =  this_error
增加迭代次数
返回 best_model, best_consensus_set, best_error

RANSAC算法的可能变化包括以下几种：
（1）如果发现了一种足够好的模型（该模型有足够小的错误率），则跳出主循环。这样可能会节约计算额外参数的时间。
（2）直接从maybe_model计算this_error，而不从consensus_set重新估计模型。这样可能会节约比较两种模型错误的时间，但可能会对噪声更敏感。

其实核心就是随机性和假设性。随机性用于减少计算了，那个循环次数就是利用正确数据出现的概率。所谓的假设性，就是说随机抽出来的数据我都认为是正确的，并以此去计算其他点，获得其他满足变换关系的点，然后利用投票机制，选出获票最多的那一个变换。

以上关于Ransac算法的内容源自：RANSAC算法讲解_fandq1223的博客-CSDN博客_ransac算法

--------------------------------------------------------------------------------------------------------------------------------

Ransac_fitline.h

#include <cmath>：三角函数、双曲函数、指数函数与对数函数、幂函数、误差与伽马函数、四舍五入与余数函数、绝对值、最小、最大

C++库文件 (math.h)_小哈龙的博客-CSDN博客_c++ cmath1、三角函数 Trigonometric functions1.1、 cos() 函数/* cos example */#include <stdio.h> /* printf */#include <math.h> /* cos */#define PI 3.14159265int main (){ double param,...https://blog.csdn.net/qq_22642239/article/details/82978347?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522165966337216781667822124%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=165966337216781667822124&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-4-82978347-null-null.142%5Ev39%5Econtrol&utm_term=%23include%20%3Ccmath%3E&spm=1018.2226.3001.4187#include <memory>： auto_ptr、unique_ptr两种指针

#include _B1334628598的博客-CSDN博客1auto_ptr2unique_ptr1auto_ptrC++的auto_ptr所做的事情，就是动态分配对象以及当对象不再需要时自动执行清理。使用std::auto_ptr，要#include <memory>。double *p = new double;//为指针分配内存std::auto_ptr<dou...https://blog.csdn.net/B1334628598/article/details/101810218?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522165966365716782248531995%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=165966365716782248531995&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~baidu_landing_v2~default-4-101810218-null-null.142%5Ev39%5Econtrol&utm_term=%23include%20%3Cmemory%3E&spm=1018.2226.3001.4187#include <omp.h>：多线程头文件、并行处理文件

#include <random>：用于生成随机数

#pragma once#include "utils/common.h"
#include <algorithm>
#include <cmath>
#include <iostream>
#include <memory>
#include <omp.h>
#include <random>
#include <string>
#include <vector>struct LineModel {// ax + by + c = 0double m_a;double m_b;double m_c;std::vector<int> m_inliner_index;LineModel() { m_inliner_index.clear(); }//inliner_index代表局内点数据组成的数组LineModel(double a, double b, double c, std::vector<int> inliner_index): m_a(a), m_b(b), m_c(c), m_inliner_index(inliner_index) {}void Clear() { m_inliner_index.clear(); }
};class RansacFitLine {
public://thresold 用于决定数据是否适应于模型的阀值RansacFitLine(double threshold, int max_iternations = 10);//构造函数~RansacFitLine(){};//析构函数  //预测一个观测模型bool Estimate(const std::vector<double> x, const std::vector<double> y,double &a, double &b, double &c,std::vector<int> &inliner_index);private://评估观测模型bool Evaluate(double a, double b, double c, double &score,std::vector<int> &inliner_idx);void getRandomIndex();void LeastSquare(const std::vector<double> x, const std::vector<double> y,double &a, double &b, double &c);void GetLineSlope(double x1, double y1, double x2, double y2, double &a,double &b, double &c);private:std::vector<double> x_;std::vector<double> y_;// std::vector<std::shared_ptr<LineModel>> line_models_;//利用智能指针，以LineModel为基础，实例化best_line_model_std::shared_ptr<LineModel> best_line_model_;//定义用于决定数据是否适用于观测模型的阈值、最大迭代次次数、最优得分double threshold_;int max_iternations_;double best_score_;// int best_model_idx_;std::vector<std::mt19937> rand_engines_;
};

Ransac_fitline.cpp：

srand(time(NULL))：这是两个函数，其中srand函数是在调用rand()函数之前使用的，rand()时一个产生随机数的函数，而srand是一个设置随机数种子的函数，通常这两个函数时一起使用的，实现产生随机数。 time函数：time(BULL)这个函数的返回值是作为srand函数的参数的：意思是以系统现在的时间作为随机数的种子来产生随机数。至于NULL这个参数，只有设置成NULL 才能获得系统的时间。

shared_ptr：智能指针，用于动态资源管理，为数据自动分配空间，当数据不在作用域时，自动删除数据所在的空间。构造时常与make_shared连用

C++智能指针详解：shared_ptr_吃素的施子的博客-CSDN博客_c++ shared_ptrC++没有内存回收机制，每次程序员new出来的对象需要手动delete，流程复杂时可能会漏掉delete，导致内存泄漏。于是C++引入智能指针，可用于动态资源管理，资源即对象的管理策略。使用 raw pointer 管理动态内存时，经常会遇到这样的问题：忘记delete内存，造成内存泄露。出现异常时，不会执行delete，造成内存泄露。下面的代码解释了，当一个操作发生异常时，会导致delete不会被执行：void func(){ auto ptr = new Widget;https://blog.csdn.net/feikudai8460/article/details/122023528?ops_request_misc=&request_id=&biz_id=102&utm_term=shared_ptr&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-1-122023528.142%5Ev39%5Econtrol&spm=1018.2226.3001.4187

#include "utils/Ransac_fitline.h"#include <stdlib.h>//实现RansacFitLine类中的RansacFitLine（）函数，传入的数据为判断数据点是否适用于观测模型的阈值和最大迭代次数
RansacFitLine::RansacFitLine(double threshold, int max_iternations) {threshold_ = threshold;max_iternations_ = max_iternations;// TODO: add omp// int nThreads = std::max(1, omp_get_max_threads());// std::cout << "[ INFO ]: Maximum usable threads: " << nThreads << std::endl;// for (int i = 0; i < nThreads; ++i)// {//  std::random_device SeedDevice;//    rand_engines_.push_back(std::mt19937(SeedDevice()));// }best_score_ = 0;
}//实现RansacFitLine类中Estimate（）函数
bool RansacFitLine::Estimate(const std::vector<double> x,const std::vector<double> y, double &a, double &b,double &c, std::vector<int> &inliner_index) {x_ = x;y_ = y;/* generate random index vector */srand(time(NULL));// rand_idx_vec表示随机数组std::vector<int> rand_idx_vec;int idx1, idx2;//这个for循环实现的功能：产生适用于(x,y)数据的随机数，放到一个数组中，后面搭建观测模型时，做到随机选取(X,y)点对for (int i = 0; i < max_iternations_; i++) {//random()函数用于产生随机数idx1 = random() % x.size();idx2 = random() % x.size();while (idx2 == idx1) {idx2 = random() % x.size();}rand_idx_vec.push_back(idx1);rand_idx_vec.push_back(idx2);}// int nThreads = std::max(1, omp_get_max_threads());// omp_set_dynamic(0); // Explicitly disable dynamic teams// omp_set_num_threads(nThreads);// #pragma omp parallel for//这个for循环实现的功能是：利用数组中的两对(x,y)点调用GetLineSlope（）函数生成观测模型，然后调用Evaluate（）函数对于得到的观测模型进行评估，一直for循环这个过程，直到到达允许的最大迭代次数for (int i = 0; i < max_iternations_; i++) {idx1 = rand_idx_vec[i * 2];idx2 = rand_idx_vec[i * 2 + 1];// std::cout << "=>Iter " << i << " (" << idx1 << ", " << idx2 << ")\n";//利用智能指针shared_ptr以LineModel类为基础，实例化出对象line_model（为指针），<>内为数据类型，构造智能指针shared_ptr与make_shared连用，可以避免错误的出现std::shared_ptr<LineModel> line_model = std::make_shared<LineModel>();//this指代上面创建的智能指针line_model，调用GetLineSlope()函数，实现由两点创建一条直线的函数形式this->GetLineSlope(x_[idx1], y_[idx1], x_[idx2], y_[idx2], line_model->m_a,line_model->m_b, line_model->m_c);double score;//对由两点创建的直线函数进行评估this->Evaluate(line_model->m_a, line_model->m_b, line_model->m_c, score,line_model->m_inliner_index);//所得评分满足条件，进行最优得分和最优模型的估计if (score > best_score_) {// std::cout << "best score: " << score << std::endl;best_score_ = score;best_line_model_ = line_model;}}//inliner_index指由若干局内点构成的数组inliner_index = best_line_model_->m_inliner_index;a = best_line_model_->m_a;b = best_line_model_->m_b;c = best_line_model_->m_c;return true;
}//实现RansacFitLine类中的Evaluate（）函数，实现对观测模型的评估
bool RansacFitLine::Evaluate(double a, double b, double c, double &score,std::vector<int> &inliner_idx) {if (a == 0 && b == 0) {std::cerr << "[RansacFitLine]Wrong line param." << std::endl;return false;}inliner_idx.clear();int inliner_num = 0;int total_num = x_.size();double line_dist_sqrt = sqrt(a * a + b * b);for (int i = 0; i < total_num; i++) {double cur_x = x_[i];double cur_y = y_[i];// compute distance between point and line，计算点到线的距离来判断是否属于局内点，如果属于，进行原局内点数组的扩展//fabs()函数是返回某一个值的绝对值的一个函数double dist_value = fabs(a * cur_x + b * cur_y + c) / line_dist_sqrt;if (dist_value <= threshold_) {inliner_idx.push_back(i);inliner_num++;}}// std::cout << "inliner num: " << inliner_num << std::endl;//static_cast进行数据类型的强制转换，计算得分：满足模型的数据点(inliner_num)/所有的数据点(total_num)score = static_cast<double>(inliner_num) / static_cast<double>(total_num);return true;
}// ax + by + c = 0
//实现RansacFitLine类中的LeastSquare()函数，由给出的一系列(x,y)点确定一条直线
void RansacFitLine::LeastSquare(const std::vector<double> x,const std::vector<double> y, double &a,double &b, double &c) {double t1 = 0, t2 = 0, t3 = 0, t4 = 0;for (size_t i = 0; i < x.size(); ++i) {t1 += x[i] * x[i];t2 += x[i];t3 += x[i] * y[i];t4 += y[i];}if (t1 * x.size() == t2 * t2) {b = 0;a = 1;c = -x[0];} else {a = -(t3 * x.size() - t2 * t4) / (t1 * x.size() - t2 * t2);b = 1;c = -(t1 * t4 - t2 * t3) / (t1 * x.size() - t2 * t2);}
}//实现RansacFitLine类中的GetLineSlope（）函数，由两点确定一条直线
void RansacFitLine::GetLineSlope(double x1, double y1, double x2, double y2,double &a, double &b, double &c) {if (x1 == x2) {b = 0;a = 1;c = -x1;} else {a = -(y1 - y2) / (x1 - x2);b = 1;c = -(x1 * y2 - x2 * y1) / (x1 - x2);}
}

Imu_heading源码阅读（三）——Ransac_fitline部分相关推荐

24 UsageEnvironment使用环境抽象基类——Live555源码阅读(三)UsageEnvironment
24 UsageEnvironment使用环境抽象基类--Live555源码阅读(三)UsageEnvironment 24 UsageEnvironment使用环境抽象基类--Live555源码阅读 ...
mybatis源码阅读(三)：mybatis初始化（下）mapper解析
转载自 mybatis源码阅读(三):mybatis初始化(下)mapper解析 MyBatis 的真正强大在于它的映射语句,也是它的魔力所在.由于它的异常强大,映射器的 XML 文件就显得相对简单. ...
SDWebImage源码阅读(三)UIImage+GIF
UIImage+GIF 是UIImage 类的一个GIF 分类,在之前的版本里面这个分类是用了处理GIF 动态图片的但是会有内存暴增的bug.在当前 '4.0.0-beta2' 的版本里GIF 动态图 ...
Imu_heading源码阅读（四）——具体实现
imu_heading.hpp: #pragma once一般由编译器提供保证:同一个文件不会被包含多次.这里所说的"同一个文件"是指物理上的一个文件,而不是指内容相同的两个文件. ...
Struts2源码阅读(三)_DispatcherConfigurationProvider
首先强调一下struts2的线程程安全,在Struts2中大量采用ThreadLocal线程局部变量的方法来保证线程的安全,像Dispatcher等都是通过ThreadLocal来保存变量值,使得每个 ...
SpringMVC源码阅读(三)
先理一下Bean的初始化路线 org.springframework.beans.factory.support.AbstractBeanDefinitionReader public int loa ...
Imu_heading源码阅读（二）——GPS_time部分
GPS部分:主要进行时间格式上的转换,将utc时间转换为unix时间(以秒为单位,具有时间戳) GPS_time.h: #pragma once一般由编译器提供保证:同一个文件不会被包含多次.这里所说 ...
redis源码阅读-持久化之RDB
持久化介绍: redis的持久化有两种方式: rdb :可以在指定的时间间隔内生成数据集的时间点快照(point-in-time snapshot) aof : 记录redis执行的所有写操作命令根 ...
redis源码阅读-zset
前段时间给小伙伴分享redis,顺带又把redis撸了一遍了,对其源码,又有了比较深入的了解.(ps: 分享的文章再丰富下再放出来). 数据结构我们先看下redis 5.0的代码.本次讲解主要是zs ...

Imu_heading源码阅读（三）——Ransac_fitline部分

Imu_heading源码阅读（三）——Ransac_fitline部分相关推荐

最新文章

热门文章