目录
先对特征点提取和优化过程进行了梳理，见博客：
cam_lidar_calibration代码详解（一）采样优化部分_可见一班的博客-CSDN博客
这篇博客主要对优化计算的过程及代码进行整理，用到的是遗传算法。主要包含3个文件：
- openga.h，遗传算法的实现
- optimiser.h，定义旋转矩阵、变化矩阵优化计算用到的函数
- optimiser.cpp，执行遗传算法的配置和流程
一、openga.h解析
是一个遗传算法开源库，有三种计算模式，使用的是SOGA模式
enum class GA_MODE
{
SOGA,
IGA,
NSGA_III
};定义染色体结构，ChromosomeType放在GenerationType里，又放在GenerationType_SO_abstract里
template <typename GeneType, typename MiddleCostType>
struct ChromosomeType
{
GeneType genes;
MiddleCostType middle_costs; // individual costs
double total_cost; // for single objective
vector<double> objectives; // for multi-objective
};
template <typename GeneType, typename MiddleCostType>
struct GenerationType
{
vector<ChromosomeType<GeneType, MiddleCostType>> chromosomes;
double best_total_cost = (std::numeric_limits<double>::infinity()); // for single objective
double average_cost = 0.0; // for single objective
int best_chromosome_index = -1; // for single objective
vector<int> sorted_indices; // for single objective
vector<vector<unsigned int>> fronts; // for multi-objective
vector<double> selection_chance_cumulative;
double exe_time;
};
template <typename GeneType, typename MiddleCostType>
struct GenerationType_SO_abstract
{
double best_total_cost = (std::numeric_limits<double>::infinity()); // for single objective
double average_cost = 0.0; // for single objective
GenerationType_SO_abstract(const GenerationType<GeneType, MiddleCostType>& generation)
: best_total_cost(generation.best_total_cost), average_cost(generation.average_cost)
{
}
};对矩阵进行操作的一些函数，如置零、清除、获取行数列数等，在矩阵类Matrix中
class Matrix
{
unsigned int n_rows, n_cols;
vector<double> data;
public:
//这是一个构造函数，用于初始化一个名为Matrix的类。其中，n_rows和n_cols是表示矩阵的行数和列数的变量，data是一个指向矩阵数据的指针。在这个构造函数中，n_rows和n_cols被初始化为0，data被初始化为空指针，即还没有分配矩阵数据的内存空间。
Matrix() : n_rows(0), n_cols(0), data()
{
}
//这也是一个构造函数，用于初始化一个名为Matrix的类，并指定矩阵的行数和列数。在这个构造函数中，n_rows和n_cols被初始化为传入的参数n_rows和n_cols，data被初始化为一个大小为n_rows * n_cols的向量，也就是说，它为矩阵数据分配了一个连续的内存空间。这个构造函数可以用于创建一个空矩阵，也可以用于创建一个具有指定行数和列数的矩阵。
Matrix(unsigned int n_rows, unsigned int n_cols) : n_rows(n_rows), n_cols(n_cols), data(n_rows * n_cols)
{
}
void zeros() //这个函数适用于已经存在的矩阵，可以将矩阵中的所有元素清零
{
std::fill(data.begin(), data.end(), 0);
}
void zeros(unsigned int rows, unsigned int cols) //这个函数适用于创建一个新的矩阵，并将其中的所有元素初始化为0。
{
n_rows = rows;
n_cols = cols;
data.assign(rows * cols, 0); //其中"assign"函数的第一个参数是要分配的元素数量，第二个参数是要分配的元素的值。
}
bool empty()
{
return (!n_rows) || (!n_cols); //如果其中一个为0，则返回true，否则返回false。判断一个矩阵是否为空，即行数或列数是否为0。
}
//用于获取矩阵的行数和列数的。返回一个无符号整数类型的值
unsigned int get_n_rows() const
{
return n_rows;
}
unsigned int get_n_cols() const
{
return n_cols;
}
void clear()
{
n_rows = 0;
n_cols = 0;
data.clear();
}
//这段代码定义了一个名为set_col的函数，它有两个参数：col_idx和col_vector。col_idx是一个无符号整数类型的值，表示要设置的列的索引。col_vector是一个双精度浮点数向量，表示要分配给该列的值。该函数使用assert函数来确保col_vector的大小与矩阵的行数相同。然后，它使用一个for循环来遍历矩阵的每一行，并将col_vector中对应的值分配给该行的col_idx列。这个函数可能是用于设置矩阵的某一列的值的。
void set_col(unsigned int col_idx, const vector<double>& col_vector)
{
assert(col_vector.size() == n_rows && "Assigned column vector size mismatch."); //如果col_vector的大小与n_rows不同，assert函数将会抛出一个错误，提示“Assigned column vector size mismatch.”，并终止程序的执行。这个assert语句的作用是确保在设置矩阵的某一列时，分配给该列的向量的大小与矩阵的行数相同，以避免出现错误的结果。
for (unsigned int i = 0; i < n_rows; i++)
(*this)(i, col_idx) = col_vector[i];
}
void set_row(unsigned int row_idx, const vector<double>& row_vector)
{
assert(row_vector.size() == n_cols && "Assigned row vector size mismatch.");
for (unsigned int i = 0; i < n_cols; i++)
(*this)(row_idx, i) = row_vector[i];
}
void get_col(unsigned int col_idx, vector<double>& col_vector) const
{
col_vector.resize(n_rows);
for (unsigned int i = 0; i < n_rows; i++)
col_vector[i] = (*this)(i, col_idx);
}
void get_row(unsigned int row_idx, vector<double>& row_vector) const
{
row_vector.resize(n_cols);
for (unsigned int i = 0; i < n_cols; i++)
row_vector[i] = (*this)(row_idx, i);
}计时用高精度时钟类Chronometer的tic()和toc()函数
class Chronometer //高度精确的钟表
{
protected:
typedef std::chrono::time_point<std::chrono::high_resolution_clock> Timetype;
Timetype time_start, time_stop;
bool initialized;
public:
Chronometer() : initialized(false)
{
}
void tic()
{
initialized = true;
time_start = std::chrono::high_resolution_clock::now();
}
double toc()
{
if (!initialized)
throw runtime_error("Chronometer is not initialized!");
time_stop = std::chrono::high_resolution_clock::now();
return (double)std::chrono::duration<double>(time_stop - time_start).count();
}
};主要是类 Genetic，下面是各成员主要函数，protected里的函数较多
class Genetic
{
private: //私有成员,类的外部是不可访问的，只有类和友元函数可以访问私有成员
std::mt19937_64 rng; // random generator,这是一个C++标准库中的随机数生成器。它可以生成64位的随机数。
std::uniform_real_distribution<double> unif_dist; //随机数分布器,可以生成指定范围内的均匀分布的随机数
int average_stall_count;
int best_stall_count;
vector<double> ideal_objectives; // for multi-objective
Matrix extreme_objectives; // for multi-objective
vector<double> scalarized_objectives_min; // for multi-objective
Matrix reference_vectors;
// double shrink_scale;
unsigned int N_robj;
public: //公有成员,在程序类的外部可以访问
typedef ChromosomeType<GeneType, MiddleCostType> thisChromosomeType;
typedef GenerationType<GeneType, MiddleCostType> thisGenerationType;
typedef GenerationType_SO_abstract<GeneType, MiddleCostType> thisGenSOAbs;
GA_MODE problem_mode;
unsigned int population;
double crossover_fraction;
double mutation_rate;
bool verbose;
int generation_step;
int elite_count;
int generation_max;
double tol_stall_average;
int average_stall_max;
double tol_stall_best;
int best_stall_max;
unsigned int reference_vector_divisions;
bool enable_reference_vectors;
bool multi_threading;
bool dynamic_threading;
int N_threads;
bool user_request_stop;
long idle_delay_us;
function<void(thisGenerationType&)> calculate_IGA_total_fitness; //计算当前一代个体的总适应度。
function<double(const thisChromosomeType&)> calculate_SO_total_fitness;
function<vector<double>(thisChromosomeType&)> calculate_MO_objectives;
function<vector<double>(const vector<double>&)> distribution_objective_reductions;
function<void(GeneType&, const function<double(void)>& rnd01)> init_genes;
function<bool(const GeneType&, MiddleCostType&)> eval_solution;
function<bool(const GeneType&, MiddleCostType&, const thisGenerationType&)> eval_solution_IGA;
function<GeneType(const GeneType&, const function<double(void)>& rnd01, double shrink_scale)> mutate;
function<GeneType(const GeneType&, const GeneType&, const function<double(void)>& rnd01)> crossover;
function<void(int, const thisGenerationType&, const GeneType&)> SO_report_generation;
function<void(int, const thisGenerationType&, const vector<unsigned int>&)> MO_report_generation;
function<void(void)> custom_refresh;
function<double(int, const function<double(void)>& rnd01)> get_shrink_scale;
vector<thisGenSOAbs> generations_so_abs;
thisGenerationType last_generation;
Genetic();
int get_number_reference_vectors(int N_objectives, int N_divisions);
void calculate_N_robj(const thisGenerationType& g);
void solve_init();
StopReason solve_next_generation();
StopReason solve();
std::string stop_reason_to_string(StopReason stop)
protected: //受保护成员,保护成员在派生类（即子类）中是可访问的
};首先区分代码中对应的遗传算法模式，用到了两个判断项， bool is_single_objective()和is_interactive，在受保护模式中。这个标定程序用的是SOGA模式，分别对应true和false，一些判断不符合这个的代码段就不用看了，还有一个常用的verbose，默认为false
bool is_single_objective() // ture SOGA模式/IGA模式
{
switch (problem_mode)
{
case GA_MODE::SOGA:
return true;
case GA_MODE::IGA:
return true;
case GA_MODE::NSGA_III:
return false;
default:
throw runtime_error("Code should not reach here!");
}
}
bool is_interactive() //false是SOGA模式，true是IGA模式
{
switch (problem_mode)
{
case GA_MODE::SOGA:
return false;
case GA_MODE::IGA:
return true;
case GA_MODE::NSGA_III:
return false;
default:
throw runtime_error("Code should not reach here!");
}
}
定义了一个枚举类型的停止机制和判断条件
enum class StopReason
{
Undefined,
MaxGenerations,
StallAverage,
StallBest,
UserRequest
}; StopReason stop_critera()
{
if (generation_step < 2 && !user_request_stop)
return StopReason::Undefined;
if (is_single_objective())
{
const thisGenSOAbs& g1 = generations_so_abs[int(generations_so_abs.size()) - 2];
const thisGenSOAbs& g2 = generations_so_abs[int(generations_so_abs.size()) - 1];
if (std::abs(g1.best_total_cost - g2.best_total_cost) < tol_stall_best)
best_stall_count++;
else
best_stall_count = 0;
if (std::abs(g1.average_cost - g2.average_cost) < tol_stall_average)
average_stall_count++;
else
average_stall_count = 0;
}
if (generation_step >= generation_max)
return StopReason::MaxGenerations;
if (average_stall_count >= average_stall_max)
return StopReason::StallAverage;
if (best_stall_count >= best_stall_max)
return StopReason::StallBest;
if (user_request_stop)
return StopReason::UserRequest;
return StopReason::Undefined;
}遗传算法的有些代码还是看不太懂，代码量太大……后期看懂了补上。
二、optimiser.h解析
旋转矩阵定义到了Rotation结构体中，按照xyz轴顺序变化。定义的是三个姿态角，可以通过toMat函数将欧拉角转换为旋转矩阵。
注意：x轴向前，与横滚角roll对应；y轴向左，与俯仰角pitch对应；z轴向上，与航向角yaw对应。
struct Rotation
{
double roll; // Rotation optimization variables
double pitch;
double yaw;
operator const std::string() const
{
return std::string("{") + "roll:" + std::to_string(roll) + ", pitch:" + std::to_string(pitch) +
", yaw:" + std::to_string(yaw) + "}";
}
cv::Mat toMat() const
{
using cv::Mat_;
using std::cos;
using std::sin;
cv::Mat R_x = (Mat_<double>(3, 3) << 1, 0, 0, 0, cos(roll), -sin(roll), 0, sin(roll), cos(roll));
// Calculate rotation about y axis
cv::Mat R_y = (Mat_<double>(3, 3) << cos(pitch), 0, sin(pitch), 0, 1, 0, -sin(pitch), 0, cos(pitch));
// Calculate rotation about z axis
cv::Mat R_z = (Mat_<double>(3, 3) << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1);
return R_z * R_y * R_x;
}
};进一步地，以旋转矩阵Rotation为基础定义变换矩阵RotationTranslation，也就是增加了平移向量xyz
struct RotationTranslation
{
Rotation rot;
double x;
double y;
double z;
operator const std::string() const
{
return std::string("{") + "roll:" + std::to_string(rot.roll) + ", pitch:" + std::to_string(rot.pitch) +
", yaw:" + std::to_string(rot.yaw) + ", x:" + std::to_string(x) + ", y:" + std::to_string(y) +
", z:" + std::to_string(z) + "}";
}
};然后，定义两个矩阵的计算损失函数，用于存放中间计算量。
struct RotationCost // equivalent to y in matlab
{
double objective1; // This is where the results of simulation is stored but not yet finalized.
};
struct RotationTranslationCost // equivalent to y in matlab
{
double objective2; // This is where the results of simulation is stored but not yet finalized.
};下一部分，定义了存放计算量的数据结构体OptimisationSample，再以此为基础定义SetAssess
struct OptimisationSample
{
cv::Point3d camera_centre{ 0, 0, 0 };
cv::Point3d camera_normal{ 0, 0, 0 };
std::vector<cv::Point3d> camera_corners;
cv::Point3d lidar_centre{ 0, 0, 0 };
cv::Point3d lidar_normal{ 0, 0, 0 };
std::vector<cv::Point3d> lidar_corners;
std::vector<double> angles_0; // Currently unused - only for print statements on capture
std::vector<double> angles_1; // Currently unused - only for print statements on capture
std::vector<double> widths;
std::vector<double> heights;
float distance_from_origin; // Currently unused - only for print statements on capture
double pixeltometre;
int sample_num;
};
struct SetAssess
{
float voq;
std::vector<OptimisationSample> set;
};angles_0，angles_1，distance_from_origin 没有参与具体计算
定义一个类Optimiser。两套计算内容，旋转矩阵、变化矩阵。先优化求解最优旋转矩阵，再以此为初始值，优化旋转矩阵和平移向量，提高精度。设置遗传算法的适应函数、交叉、变异、评估精度、初始化基因
void SO_report_generation(int generation_number, const EA::GenerationType<Rotation, RotationCost>& last_generation, const Rotation& best_genes);
double calculate_SO_total_fitness(const GA_Rot_t::thisChromosomeType& X);
Rotation crossover(const Rotation& X1, const Rotation& X2, const std::function<double(void)>& rnd01);
Rotation mutate(const Rotation& X_base, const std::function<double(void)>& rnd01, const Rotation& initial_rotation,const double angle_increment, const double shrink_scale);
bool eval_solution(const Rotation& p, RotationCost& c);
void init_genes(Rotation& p, const std::function<double(void)>& rnd01, const Rotation& initial_rotation,double increment);定义四个损失函数及具体内容
double perpendicularCost(const Rotation& rot);
double normalAlignmentCost(const Rotation& rot);
double reprojectionCost(const RotationTranslation& rot_trans);
double centreAlignmentCost(const RotationTranslation& rot_trans);
double Optimiser::perpendicularCost(const Rotation& rot)
{
// We do all the alignment of features in the lidar frame
// Eq (3) in the original baseline paper
double cost = 0;
for (const auto& sample : current_set_)
{
auto camera_normal_lidar_frame = rot * sample.camera_normal;
auto perp = cv::Mat(sample.lidar_centre - sample.lidar_corners[0]).reshape(1);
perp /= cv::norm(perp);
cost += std::pow(perp.dot(camera_normal_lidar_frame), 2) / current_set_.size();
}
return cost;
}
double Optimiser::normalAlignmentCost(const Rotation& rot)
{
// Eq (4) in the original baseline paper
// We do all the alignment of features in the lidar frame
double cost;
for (const auto& sample : current_set_)
{
auto camera_normal_lidar_frame = rot * sample.camera_normal;
cost += cv::norm(camera_normal_lidar_frame - cv::Mat(sample.lidar_normal).reshape(1)) / current_set_.size();
}
return cost;
}
double Optimiser::reprojectionCost(const RotationTranslation& rot_trans)
{
// We do all the alignment of features in the lidar frame
cv::Mat rvec = cv::Mat_<double>::zeros(3, 1);
cv::Mat tvec = cv::Mat_<double>::zeros(3, 1);
double cost = 0;
for (auto& sample : current_set_)
{
std::vector<cv::Point3d> cam_centre_3d;
std::vector<cv::Point3d> lidar_centre_3d;
auto camera_centre_lidar_frame = cv::Point3d(rot_trans * sample.camera_centre);
cam_centre_3d.push_back(camera_centre_lidar_frame);
lidar_centre_3d.push_back(sample.lidar_centre);
std::vector<cv::Point2d> cam, lidar;
if (i_params_.fisheye_model)
{
cv::fisheye::projectPoints(cam_centre_3d, cam, rvec, tvec, i_params_.cameramat, i_params_.distcoeff);
cv::fisheye::projectPoints(lidar_centre_3d, lidar, rvec, tvec, i_params_.cameramat, i_params_.distcoeff);
}
else
{
cv::projectPoints(cam_centre_3d, rvec, tvec, i_params_.cameramat, i_params_.distcoeff, cam);
cv::projectPoints(lidar_centre_3d, rvec, tvec, i_params_.cameramat, i_params_.distcoeff, lidar);
}
double error = cv::norm(cam[0] - lidar[0])*sample.pixeltometre;
if (error > cost)
{
cost = error;
}
}
return cost;
}
double Optimiser::centreAlignmentCost(const RotationTranslation& rot_trans)
{
// Eq (6) and (7)
// We do all the alignment of features in the lidar frame
double abs_mean = 0;
for (const auto& sample : current_set_)
{
auto camera_centre_lidar_frame = rot_trans * sample.camera_centre;
abs_mean += cv::norm(camera_centre_lidar_frame - cv::Mat(sample.lidar_centre).reshape(1)) / current_set_.size();
}
double stddev = 0;
for (const auto& sample : current_set_)
{
auto camera_centre_lidar_frame = rot_trans * sample.camera_centre;
stddev += std::pow(cv::norm(camera_centre_lidar_frame - cv::Mat(sample.lidar_centre).reshape(1)) - abs_mean, 2) /
current_set_.size();
}
stddev = std::sqrt(stddev);
return abs_mean/1000 + stddev/1000;
}计算平均误差
void get_mean_stdev(std::vector<float>& input, float& mean, float& stdev);
void Optimiser::get_mean_stdev(std::vector<float>& input_vec, float& mean, float& stdev){
//这段代码使用了C++标准库中的accumulate函数，对一个浮点数向量input_vec中的所有元素进行求和，并将结果存储在sum变量中。其中，std::begin(input_vec)和std::end(input_vec)分别表示向量input_vec的起始和结束位置，0.0表示初始值为0.0
float sum = std::accumulate(std::begin(input_vec), std::end(input_vec), 0.0);
mean = sum / input_vec.size();
//这段代码使用了C++标准库中的for_each函数，对一个名为input_vec的浮点数向量进行遍历。计算了每个元素与平均值的差的平方，并将结果累加到一个名为accum的变量中。
float accum = 0.0;
std::for_each (std::begin(input_vec), std::end(input_vec), [&](const float d) {
accum += (d - mean) * (d - mean);
});
stdev = sqrt(accum / (input_vec.size()-1));
}声明旋转矩阵转换欧拉角的函数，及具体内容（在cpp中）
std::vector<double> rotm2eul(cv::Mat);
std::vector<double> rotm2eul(cv::Mat mat)
{
std::vector<double> euler(3);
euler[0] = atan2(mat.at<double>(2, 1), mat.at<double>(2, 2)); // rotation about x axis: roll
euler[1] = atan2(-mat.at<double>(2, 0),
sqrt(mat.at<double>(2, 1) * mat.at<double>(2, 1) + mat.at<double>(2, 2) * mat.at<double>(2, 2)));
euler[2] = atan2(mat.at<double>(1, 0), mat.at<double>(0, 0)); // rotation about z axis: yaw
return euler;
}三、optimiser.cpp解析
初始化第一代， roll，pitch和yaw。函数接受四个参数，其中第一个参数是一个旋转对象，后面三个参数分别是一个返回0到1之间随机数的函数、一个初始旋转对象和一个增量值（优化旋转矩阵时为pi/8，优化变换矩阵时为pi/18）。函数首先创建一个包含两个元素的double类型向量pi_vals。接着，函数分别使用随机选择的增量值和随机数函数计算出新的旋转角度，并将其赋值给旋转对象的roll、pitch和yaw属性。最终，函数返回void类型。
void Optimiser::init_genes(Rotation& p, const std::function<double(void)>& rnd01, const Rotation& initial_rotation,
double increment)
{
std::vector<double> pi_vals;
pi_vals.push_back(increment);
pi_vals.push_back(-increment);
int RandIndex = rand() % 2;
p.roll = initial_rotation.roll + pi_vals.at(RandIndex) * rnd01();
RandIndex = rand() % 2;
p.pitch = initial_rotation.pitch + pi_vals.at(RandIndex) * rnd01();
RandIndex = rand() % 2;
p.yaw = initial_rotation.yaw + pi_vals.at(RandIndex) * rnd01();
}对旋转矩阵用perpendicularCost(p) 和 normalAlignmentCost两个函数处理，见optimiser.h文件，赋值到损失函数里，返回true
bool Optimiser::eval_solution(const Rotation& p, RotationCost& c)
{
c.objective1 = perpendicularCost(p) + normalAlignmentCost(p);
return true; // solution is accepted
}变异生成一个新的旋转矩阵。函数接受四个参数：X_base表示基础旋转矩阵，rnd01是一个返回0到1之间随机数的函数，initial_rotation表示初始旋转矩阵，angle_increment表示旋转角度增量，shrink_scale表示缩小比例。函数会在一定范围内随机生成新的旋转矩阵，直到生成的旋转矩阵在指定的范围内。具体实现是通过在每个轴上随机增加一定角度，然后判断是否在指定范围内。
Rotation Optimiser::mutate(const Rotation& X_base, const std::function<double(void)>& rnd01,
const Rotation& initial_rotation, const double angle_increment, double shrink_scale)
{
Rotation X_new;
bool in_range;
do
{
in_range = true;
X_new = X_base;
float roll_inc = 0.2 * (rnd01() - rnd01()) * shrink_scale;
X_new.roll += roll_inc;
in_range = in_range && (X_new.roll >= (initial_rotation.roll - angle_increment) &&
X_new.roll < (initial_rotation.roll + angle_increment));
float pitch_inc = 0.2 * (rnd01() - rnd01()) * shrink_scale;
X_new.pitch += pitch_inc;
in_range = in_range && (X_new.pitch >= (initial_rotation.pitch - angle_increment) &&
X_new.pitch < (initial_rotation.pitch + angle_increment));
float yaw_inc = 0.2 * (rnd01() - rnd01()) * shrink_scale;
X_new.yaw += yaw_inc;
in_range = in_range && (X_new.yaw >= (initial_rotation.yaw - angle_increment) &&
X_new.yaw < (initial_rotation.yaw + angle_increment));
} while (!in_range);
return X_new;
}交叉函数，接收两个旋转矩阵X1和X2，根据两者产生X_new，X1的乘r ， X2乘（1-r）
Rotation Optimiser::crossover(const Rotation& X1, const Rotation& X2, const std::function<double(void)>& rnd01)
{
Rotation X_new;
double r = rnd01();
X_new.roll = r * X1.roll + (1.0 - r) * X2.roll;
r = rnd01();
X_new.pitch = r * X1.pitch + (1.0 - r) * X2.pitch;
r = rnd01();
X_new.yaw = r * X1.yaw + (1.0 - r) * X2.yaw;
return X_new;
}计算总的损失函数，ChromosomeType结构体
double Optimiser::calculate_SO_total_fitness(const GA_Rot_t::thisChromosomeType& X)
{
double final_cost = 0.0; // finalize the cost
final_cost += X.middle_costs.objective1;
return final_cost;
}存放当代最优矩阵 ，GenerationType结构体
void Optimiser::SO_report_generation(int generation_number,
const EA::GenerationType<Rotation, RotationCost>& last_generation,
const Rotation& best_genes)
{
best_rotation_.roll = best_genes.roll;
best_rotation_.pitch = best_genes.pitch;
best_rotation_.yaw = best_genes.yaw;
}求标准偏差，先对向量内所有元素求和，再求平均数，和每个数作差，得标准偏差、开方得stdev
void Optimiser::get_mean_stdev(std::vector<float>& input_vec, float& mean, float& stdev){
//这段代码使用了C++标准库中的accumulate函数，对一个浮点数向量input_vec中的所有元素进行求和，并将结果存储在sum变量中。其中，std::begin(input_vec)和std::end(input_vec)分别表示向量input_vec的起始和结束位置，0.0表示初始值为0.0
float sum = std::accumulate(std::begin(input_vec), std::end(input_vec), 0.0);
mean = sum / input_vec.size();
//这段代码使用了C++标准库中的for_each函数，对一个名为input_vec的浮点数向量进行遍历。计算了每个元素与平均值的差的平方，并将结果累加到一个名为accum的变量中。再除以个数，得到平均偏差stdev
float accum = 0.0;
std::for_each (std::begin(input_vec), std::end(input_vec), [&](const float d) {
accum += (d - mean) * (d - mean);
});
stdev = sqrt(accum / (input_vec.size()-1));
}从捕获的总样本生成大小为k的组合，设置总的样本数offset，每个组合的样本数k，选出的集合set，总样本集合samples，都是OptimisationSample结构体组成的vector容器
// Generate combinations of size k from the total samples captured
//这段代码是一个递归函数，用于生成从总样本中选择k个样本的所有组合。函数接受四个参数，其中第一个参数是当前已选择的样本数，第二个参数是需要选择的样本数，第三个参数是当前已选择的样本集合，第四个参数是总样本集合。函数首先判断如果需要选择的样本数为0，则将当前已选择的样本集合加入到所有组合的集合中，并返回。否则，函数使用循环从当前已选择的样本数开始，到总样本数减去需要选择的样本数为止，依次选择一个样本，并递归调用自身，将需要选择的样本数减1，当前已选择的样本集合加入该样本，然后再将该样本从当前已选择的样本集合中移除。最终，函数将生成的所有组合存储在一个集合中。
void Optimiser::generate_sets(int offset, int k, std::vector<OptimisationSample>& set, std::vector<OptimisationSample>& samples) {
if (k == 0) {
sets.push_back(set);
return;
}
for (int i = offset; i <= samples.size() - k; ++i) {
set.push_back(samples[i]);
generate_sets(i+1, k-1, set, samples);
set.pop_back();
}
}旋转矩阵部分计算完成，接下来是变换矩阵，主要分析相对于旋转矩阵增加的部分：
- init_genes里，旋转矩阵的angle_increment角度增量由pi/8改为pi/18，translation_increment位移增量为0.05*1000。
- eval_solution里，旋转矩阵部分的误差函数一样，计算位移误差用到了另外两个函数centreAlignmentCost和reprojectionCost，总误差为4项的和，保存到objective2
- mutate里，xyz的变换量有个*1000，是转换到m，增加增量，不乘的话变化量太小。
- crossover里，与旋转矩阵的一样
- calculate_SO_total_fitness里，对所有的objective2求和，为final_cost
- SO_report_generation里，将包括xyz在内的最优变换矩阵保存到best_rotation_translation_
optimise求解函数
优化旋转矩阵和变换矩阵的遗传算法配置是一样的，由openga.h里的solve执行计算，包括solve_init和solve_next_generation。
GA_Rot_t ga_obj;
ga_obj.problem_mode = EA::GA_MODE::SOGA;
ga_obj.multi_threading = false;
ga_obj.verbose = false;
ga_obj.population = 200;
ga_obj.generation_max = 1000;
ga_obj.calculate_SO_total_fitness = [&](const GA_Rot_t::thisChromosomeType& X) -> double {
return this->calculate_SO_total_fitness(X);
};
ga_obj.init_genes = [&, initial_rotation, rotation_increment](Rotation& p,
const std::function<double(void)>& rnd01) -> void {
this->init_genes(p, rnd01, initial_rotation, rotation_increment);
};
ga_obj.eval_solution = [&](const Rotation& r, RotationCost& c) -> bool { return this->eval_solution(r, c); };
ga_obj.mutate = [&, initial_rotation, rotation_increment](
const Rotation& X_base, const std::function<double(void)>& rnd01, double shrink_scale) -> Rotation {
return this->mutate(X_base, rnd01, initial_rotation, rotation_increment, shrink_scale);
};
ga_obj.crossover = [&](const Rotation& X1, const Rotation& X2, const std::function<double(void)>& rnd01) {
return this->crossover(X1, X2, rnd01);
};
ga_obj.SO_report_generation = [&](int generation_number,
const EA::GenerationType<Rotation, RotationCost>& last_generation,
const Rotation& best_genes) -> void {
this->SO_report_generation(generation_number, last_generation, best_genes);
};
ga_obj.best_stall_max = 100;
ga_obj.average_stall_max = 100;
ga_obj.tol_stall_average = 1e-8;
ga_obj.tol_stall_best = 1e-8;
ga_obj.elite_count = 10;
ga_obj.crossover_fraction = 0.8;
ga_obj.mutation_rate = 0.2;
ga_obj.best_stall_max = 10;
ga_obj.elite_count = 10;
ga_obj.solve(); tf::Matrix3x3 rot;
rot.setRPY(best_rotation_.roll, best_rotation_.pitch, best_rotation_.yaw);
cv::Mat tmp_rot = (cv::Mat_<double>(3, 3) << rot.getRow(0)[0], rot.getRow(0)[1], rot.getRow(0)[2], rot.getRow(1)[0],
rot.getRow(1)[1], rot.getRow(1)[2], rot.getRow(2)[0], rot.getRow(2)[1], rot.getRow(2)[2]);
// Analytical Translation
cv::Mat cp_trans = tmp_rot * camera_centres_.t();
cv::Mat trans_diff = lidar_centres_.t() - cp_trans;
cv::Mat summed_diff;
cv::reduce(trans_diff, summed_diff, 1, CV_REDUCE_SUM, CV_64F);
summed_diff = summed_diff / trans_diff.cols;
const RotationTranslation initial_rotation_translation{ best_rotation_, summed_diff.at<double>(0),
summed_diff.at<double>(1), summed_diff.at<double>(2) };
rotation_increment = M_PI / 18;
constexpr double translation_increment = 0.05*1000;
// extrinsics stored the vector of extrinsic parameters in every iteration
std::vector<std::vector<double>> extrinsics;
// Joint optimization for Rotation and Translation (Perform this 10 times and take the average of the extrinsics)
GA_Rot_Trans_t ga_rot_trans;
ga_rot_trans.problem_mode = EA::GA_MODE::SOGA;
ga_rot_trans.multi_threading = false;
ga_rot_trans.verbose = false;
ga_rot_trans.population = 200;
ga_rot_trans.generation_max = 1000;
ga_rot_trans.calculate_SO_total_fitness = [&](const GA_Rot_Trans_t::thisChromosomeType& X) -> double {
return this->calculate_SO_total_fitness(X);
};
ga_rot_trans.init_genes = [&, initial_rotation_translation, rotation_increment, translation_increment](
RotationTranslation& p, const std::function<double(void)>& rnd01) -> void {
this->init_genes(p, rnd01, initial_rotation_translation, rotation_increment, translation_increment);
};
ga_rot_trans.eval_solution = [&](const RotationTranslation& rt, RotationTranslationCost& c) -> bool {
return this->eval_solution(rt, c);
};
ga_rot_trans.mutate = [&, initial_rotation_translation, rotation_increment, translation_increment](
const RotationTranslation& X_base, const std::function<double(void)>& rnd01,
double shrink_scale) -> RotationTranslation {
return this->mutate(X_base, rnd01, initial_rotation_translation, rotation_increment, translation_increment,
shrink_scale);
};
ga_rot_trans.crossover = [&](const RotationTranslation& X1, const RotationTranslation& X2,
const std::function<double(void)>& rnd01) { return this->crossover(X1, X2, rnd01); };
ga_rot_trans.SO_report_generation =
[&](int generation_number,
const EA::GenerationType<RotationTranslation, RotationTranslationCost>& last_generation,
const RotationTranslation& best_genes) -> void {
this->SO_report_generation(generation_number, last_generation, best_genes);
};
ga_rot_trans.best_stall_max = 100;
ga_rot_trans.average_stall_max = 100;
ga_rot_trans.tol_stall_average = 1e-8;
ga_rot_trans.tol_stall_best = 1e-8;
ga_rot_trans.elite_count = 10;
ga_rot_trans.crossover_fraction = 0.8;
ga_rot_trans.mutation_rate = 0.2;
ga_rot_trans.best_stall_max = 10;
ga_rot_trans.elite_count = 10;
ga_rot_trans.solve();
324
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
