CMakeLists.txt
cmake_minimum_required(VERSION 3.1)
project(untitled2)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_BUILD_TYPE Release)
set(ALL_TARGET_LIBRARIES "")
include(cmake/FindG2O.cmake)
#方式1
find_package(Eigen3 REQUIRED)
include_directories("/usr/local/include/eigen3")
#方式2
#include(cmake/FindEigen3.cmake)
add_executable(fit_curve fit_curve.cpp)
target_link_libraries(fit_curve ${ALL_TARGET_LIBRARIES})
fit_curve.cpp
#include <Eigen/Core>
#include <iostream>
#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/core/robust_kernel_impl.h"
using namespace std;
/*!
* 继承BaseVertex类，构造顶点
*/
class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexParams() = default;
bool read(std::istream & /*is*/) override {
cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
return false;
}
bool write(std::ostream & /*os*/) const override {
cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
return false;
}
//该函数作用是更新顶点的估计值
void setToOriginImpl() override {
cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
}
//更新优化之后的顶点
void oplusImpl(const double *update) override {
Eigen::Vector3d::ConstMapType v(update);
_estimate += v;
}
};
/*!
* 从BaseUnaryEdge继承得到一元边
*/
class EdgePointOnCurve : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgePointOnCurve() = default;
bool read(std::istream & /*is*/) override {
cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
return false;
}
bool write(std::ostream & /*os*/) const override {
cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
return false;
}
//边的误差计算
void computeError() override {
const VertexParams *params = dynamic_cast<const VertexParams *>(vertex(0));//顶点
const double &a = params->estimate()(0);
const double &b = params->estimate()(1);
const double &lambda = params->estimate()(2);
double fval = a * exp(-lambda * measurement()(0)) + b;
_error(0) = std::abs(fval - measurement()(1));
}
};
int main(int argc, char **argv) {
int numPoints = 50;
int maxIterations = 50;
bool verbose = true;
double a = 2.;
double b = 0.4;
double lambda = 0.2;
Eigen::Vector2d *points = new Eigen::Vector2d[numPoints];
ofstream points_file("../points.txt", ios::out);
//准备用于拟合的数据
for (int i = 0; i < numPoints; ++i) {
double x = g2o::Sampler::uniformRand(0, 10);
double y = a * exp(-lambda * x) + b;
y += g2o::Sampler::gaussRand(0, 0.02);
if (i == 20) {
x = 8;
y = 2.5;
}
points[i].x() = x;
points[i].y() = y;
points_file << x << " " << y << endl;
}
points_file.close();
typedef g2o::BlockSolver<g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > MyBlockSolver;
typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;
g2o::SparseOptimizer optimizer;
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<MyBlockSolver>(g2o::make_unique<MyLinearSolver>()));
optimizer.setAlgorithm(solver);
VertexParams *params = new VertexParams();
params->setId(0);
params->setEstimate(Eigen::Vector3d(1, 1, 1));//初始化顶点的估计值
optimizer.addVertex(params);
for (int i = 0; i < numPoints; ++i) {
EdgePointOnCurve *e = new EdgePointOnCurve;
e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
if (i == 20) {
e->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 10);
}
e->setVertex(0, params);
e->setMeasurement(points[i]);
g2o::RobustKernelHuber *robust_kernel_huber = new g2o::RobustKernelHuber;
robust_kernel_huber->setDelta(0.3);
e->setRobustKernel(robust_kernel_huber);
optimizer.addEdge(e);
}
optimizer.initializeOptimization();
optimizer.setVerbose(verbose);
optimizer.optimize(maxIterations);
ofstream result_file("../result.txt");
result_file << params->estimate()[0] << " "
<< params->estimate()[1] << " "
<< params->estimate()[2];
result_file.close();
cout << endl << "a, b, lambda: "
<< params->estimate()[0] << ", "
<< params->estimate()[1] << ", "
<< params->estimate()[2] << endl;
delete[] points;
return 0;
}
FindG2O.cmake
找到g2o
# Find the header files
find_path(G2O_INCLUDE_DIR g2o/core/base_vertex.h
${G2O_ROOT}/include
$ENV{G2O_ROOT}/include
$ENV{G2O_ROOT}
/usr/local/include
/usr/include
/opt/local/include
/sw/local/include
/sw/include
NO_DEFAULT_PATH
)
# Macro to unify finding both the debug and release versions of the
# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
# macro.
macro(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
find_library("${MYLIBRARY}_DEBUG"
NAMES "g2o_${MYLIBRARYNAME}_d"
PATHS
${G2O_ROOT}/lib/Debug
${G2O_ROOT}/lib
$ENV{G2O_ROOT}/lib/Debug
$ENV{G2O_ROOT}/lib
NO_DEFAULT_PATH
)
find_library("${MYLIBRARY}_DEBUG"
NAMES "g2o_${MYLIBRARYNAME}_d"
PATHS
~/Library/Frameworks
/Library/Frameworks
/usr/local/lib
/usr/local/lib64
/usr/lib
/usr/lib64
/opt/local/lib
/sw/local/lib
/sw/lib
)
find_library(${MYLIBRARY}
NAMES "g2o_${MYLIBRARYNAME}"
PATHS
${G2O_ROOT}/lib/Release
${G2O_ROOT}/lib
$ENV{G2O_ROOT}/lib/Release
$ENV{G2O_ROOT}/lib
NO_DEFAULT_PATH
)
find_library(${MYLIBRARY}
NAMES "g2o_${MYLIBRARYNAME}"
PATHS
~/Library/Frameworks
/Library/Frameworks
/usr/local/lib
/usr/local/lib64
/usr/lib
/usr/lib64
/opt/local/lib
/sw/local/lib
/sw/lib
)
if (NOT ${MYLIBRARY}_DEBUG)
if (MYLIBRARY)
set(${MYLIBRARY}_DEBUG ${MYLIBRARY})
endif (MYLIBRARY)
endif (NOT ${MYLIBRARY}_DEBUG)
endmacro(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
# Find the core elements
FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
# Find the CLI library
FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
# Find the pluggable solvers
FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
# Find the predefined types
FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
# G2O solvers declared found if we found at least one solver
set(G2O_SOLVERS_FOUND "NO")
if (G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
set(G2O_SOLVERS_FOUND "YES")
endif (G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
# G2O itself declared found if we found the core libraries and at least one solver
set(G2O_FOUND "NO")
if (G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
set(G2O_FOUND "YES")
endif (G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
include_directories(SYSTEM ${G2O_INCLUDE_DIR})
list(APPEND ALL_TARGET_LIBRARIES
${G2O_TYPES_DATA}
${G2O_CORE_LIBRARY}
${G2O_STUFF_LIBRARY}
${G2O_SOLVER_PCG}
${G2O_SOLVER_CSPARSE}
${G2O_SOLVER_CHOLMOD}
${G2O_TYPES_SLAM3D}
${G2O_TYPES_SLAM3D_ADDONS})
可视化代码
import numpy as np
import matplotlib.pyplot as plt
filename = './points.txt'
X, Y = [], []
with open(filename, 'r') as f:
lines = f.readlines()
for line in lines:
value = [float(s) for s in line.split()]
X.append(float(value[0]))
Y.append(float(value[1]))
result_name = './result.txt'
with open(result_name, 'r') as r:
lines = r.readlines()
for line in lines:
value = [float(s) for s in line.split()]
a = float(value[0])
b = float(value[1])
my_lambda = float(value[2])
x = np.linspace(0, 10, 10)
y = a * np.exp(-my_lambda * x) + b
plt.plot(x, y, 'r')
plt.scatter(X, Y)
plt.show()
