#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/slam3d/vertex_se3.h> // 关键修改：替换VertexSE3Quat
#include <g2o/types/slam3d/vertex_pointxyz.h>
#include <g2o/types/slam3d/edge_se3.h> // EdgeSE3 定义
#include <g2o/core/eigen_types.h> // Matrix6d 定义
#include <g2o/types/slam3d/types_slam3d.h> // SE3 操作
#include <opencv2/opencv.hpp>
using namespace std;
using namespace Eigen;
using Matrix6d = Eigen::Matrix<double, 6, 6>;
// 观测边定义：连接位姿顶点和路标点顶点
class EdgeObs : public g2o::BaseBinaryEdge<3, Vector3d, g2o::VertexSE3, g2o::VertexPointXYZ> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void computeError() override {
// 获取两个顶点
const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
const g2o::VertexPointXYZ* v2 = static_cast<const g2o::VertexPointXYZ*>(_vertices[1]);
// 计算误差：观测值减去估计值
Vector3d estimate = v1->estimate().inverse() * v2->estimate();
Vector3d obs(_measurement);
_error = obs - estimate;
}
virtual bool read(istream& is) override { return true; }
virtual bool write(ostream& os) const override { return true; }
};
// 定义 Deg2Rad 函数
double Deg2Rad(double deg) {
return deg * M_PI / 180.0;
}
void drawTrajectoryAndObstacles(const vector<g2o::VertexSE3*>& trajectoryVertices, const vector<g2o::VertexPointXYZ*>& obstacleVertices, const string& title) {
cv::Mat img(800, 800, CV_8UC3, cv::Scalar(255, 255, 255));
for (const auto& vertex : obstacleVertices) {
Vector3d pos = vertex->estimate();
cv::circle(img, cv::Point((pos.x() + 4.0) * 100, (-pos.y() + 4.0) * 100), 5, cv::Scalar(0, 0, 255), -1);
}
for (size_t i = 0; i < trajectoryVertices.size(); ++i) {
Vector3d pos = trajectoryVertices[i]->estimate().translation();
if (i > 0) {
Vector3d prevPos = trajectoryVertices[i-1]->estimate().translation();
cv::line(img, cv::Point((prevPos.x() + 4.0) * 100, (-prevPos.y() + 4.0) * 100),
cv::Point((pos.x() + 4.0) * 100, (-pos.y() + 4.0) * 100), cv::Scalar(0, 255, 0), 2);
}
cv::circle(img, cv::Point((pos.x() + 4.0) * 100, (-pos.y() + 4.0) * 100), 3, cv::Scalar(0, 0, 0), -1);
}
cv::imshow(title, img);
cv::waitKey(1);
}
int main() {
// 创建优化器
g2o::SparseOptimizer optimizer;
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType; // 位姿6维，路标点3维
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
// 使用智能指针避免内存泄漏
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
optimizer.setAlgorithm(solver);
// 参数设置
int numTrajectoryPoints = 4; // 轨迹点数量（正方形的四个角）
int numObstacles = 10; // 障碍物数量
// 1. 添加轨迹顶点（位姿顶点）
vector<g2o::VertexSE3*> trajectoryVertices(numTrajectoryPoints);
double sideLength = 4.0; // 正方形的边长
for (int i = 0; i < numTrajectoryPoints; ++i) {
auto vertex = new g2o::VertexSE3(); // 使用VertexSE3替代VertexSE3Quat
vertex->setId(i);
// 设置初始估计值（沿着x轴平移）
Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // 使用Eigen::Isometry3d
switch (i) {
case 0: T.translation() = Vector3d(0, 0, 0); break;
case 1: T.translation() = Vector3d(sideLength, 0, 0); break;
case 2: T.translation() = Vector3d(sideLength, sideLength, 0); break;
case 3: T.translation() = Vector3d(0, sideLength, 0); break;
}
T.rotate(AngleAxisd(Deg2Rad(rand() % 2), Vector3d::UnitZ())); // 加入角度误差
T.translate(Vector3d(rand() / double(RAND_MAX) * 0.1, rand() / double(RAND_MAX) * 0.1, 0)); // 加入位置误差
vertex->setEstimate(T);
optimizer.addVertex(vertex);
trajectoryVertices[i] = vertex;
}
// 2. 添加障碍物顶点（路标点顶点）
vector<g2o::VertexPointXYZ*> obstacleVertices(numObstacles);
for (int i = 0; i < numObstacles; ++i) {
auto vertex = new g2o::VertexPointXYZ();
vertex->setId(numTrajectoryPoints + i);
// 随机生成障碍物位置
Vector3d pos = Vector3d::Random() * 2.0; // 在[-2, 2]范围内随机生成
pos.z() = 0; // 假设在二维平面上
vertex->setEstimate(pos);
vertex->setMarginalized(true); // 路标点需要设置边缘化
optimizer.addVertex(vertex);
obstacleVertices[i] = vertex;
}
// 3. 添加轨迹边（位姿之间的边）
for (int i = 0; i < numTrajectoryPoints; ++i) {
// 使用内置的EdgeSE3代替自定义边
auto edge = new g2o::EdgeSE3();
edge->setVertex(0, trajectoryVertices[i]);
edge->setVertex(1, trajectoryVertices[(i+1)%numTrajectoryPoints]);
edge->setMeasurement(Eigen::Isometry3d::Identity()); // 使用Eigen::Isometry3d
Matrix6d information = Matrix6d::Identity();
edge->setInformation(information);
optimizer.addEdge(edge);
}
// 4. 添加观测边
// 模拟观测数据：每个观测由（轨迹点ID，障碍物ID，观测值）组成
struct Observation {
int trajectoryId;
int obstacleId;
Vector3d measurement;
};
vector<Observation> observations;
// 随机生成观测
for (int i = 0; i < numObstacles; ++i) {
for (int j = 0; j < numTrajectoryPoints; ++j) {
Observation obs;
obs.trajectoryId = j;
obs.obstacleId = i;
Vector3d pos = obstacleVertices[i]->estimate();
Vector3d trajPos = trajectoryVertices[j]->estimate().translation();
obs.measurement = pos - trajPos + Vector3d(rand() / double(RAND_MAX) * 0.1, rand() / double(RAND_MAX) * 0.1, 0); // 加入观测误差
observations.push_back(obs);
}
}
for (const auto& obs : observations) {
auto edge = new EdgeObs();
edge->setVertex(0, trajectoryVertices[obs.trajectoryId]);
edge->setVertex(1, obstacleVertices[obs.obstacleId]);
edge->setMeasurement(obs.measurement);
Matrix3d information = Matrix3d::Identity();
edge->setInformation(information);
optimizer.addEdge(edge);
}
// 5. 添加先验边（固定第一个位姿）
g2o::VertexSE3* firstVertex = trajectoryVertices[0];
firstVertex->setFixed(true); // 直接固定顶点，无需额外边
// 绘制优化前的轨迹和障碍物
drawTrajectoryAndObstacles(trajectoryVertices, obstacleVertices, "Before Optimization");
// 6. 优化
optimizer.initializeOptimization();
optimizer.setVerbose(true);
optimizer.optimize(10);
// 绘制优化后的轨迹和障碍物
drawTrajectoryAndObstacles(trajectoryVertices, obstacleVertices, "After Optimization");
// 7. 输出优化后的结果
for (int i = 0; i < numTrajectoryPoints; ++i) {
g2o::VertexSE3* v = trajectoryVertices[i];
Eigen::Isometry3d pose = v->estimate();
cout << "Trajectory Point " << i << ": " << pose.translation().transpose() << endl;
}
cv::waitKey(0);
return 0;
} void INS::optimizeTrajectory(std::vector<Eigen::Vector3d> &position)
{
std::vector<Edge> edgeData;
for (size_t i = 0; i < position.size() - 1; ++i)
{
Edge edge_data;
edge_data.s = i;
edge_data.e = i + 1;
Eigen::Vector2d d_position;
d_position(0)= position[i + 1](0) - position[i](0);
d_position(1)= position[i + 1](1) - position[i](1);
edge_data.pose = d_position;
edgeData.push_back(edge_data);
}
// 添加最后一个点到第一个点的边
Edge edge_data;
edge_data.s = position.size() - 1; // 最后一个点的索引
edge_data.e = 0; // 第一个点的索引
Eigen::Vector2d error_position(position[edge_data.s](0), position[edge_data.s](1));
Eigen::Vector2d true_position;
true_position(0) = error_position(0) + calibration_error_.Get()(0);
true_position(1) = error_position(1) + calibration_error_.Get()(1);
LogInfo("The map optimization error is = " << calibration_error_.Get()(0) << ", " << calibration_error_.Get()(1));
Eigen::Vector2d d_position;
d_position(0) = position[edge_data.e](0) - true_position(0); // e - s
d_position(1) = position[edge_data.e](1) - true_position(1);
edge_data.pose = d_position;
edgeData.push_back(edge_data);
std::unique_ptr<SlamLinearSolver> linearSolver = std::make_unique<SlamLinearSolver>();
linearSolver->setBlockOrdering(false);
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(std::make_unique<SlamBlockSolver>(std::move(linearSolver)));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
optimizer.setVerbose(false);
for (int i = 0; i < position.size(); i++)
{
g2o::VertexPointXY *v = new g2o::VertexPointXY();
v->setId(i);
v->setEstimate(g2o::Vector2());
if (i == 0)
{
v->setEstimate(g2o::Vector2(position[i](0), position[i](1)));
v->setFixed(true);
}
optimizer.addVertex(v);
}
for (const auto &pData : edgeData)
{
g2o::EdgePointXY *edge = new g2o::EdgePointXY();
edge->setVertex(0, optimizer.vertex(pData.s));
edge->setVertex(1, optimizer.vertex(pData.e));
if (&pData == &edgeData.back())
{
LogWarn("*******&pData == &edgeData.back()************");
edge->setInformation(Eigen::Matrix<double, 2, 2>::Identity()*100000);
}
else
{
edge->setInformation(Eigen::Matrix<double, 2, 2>::Identity()*0.001);
}
// edge->setInformation(Eigen::Matrix<double, 2, 2>::Identity());
edge->setMeasurement(pData.pose);
optimizer.addEdge(edge);
}
optimizer.initializeOptimization();
g2o::SparseOptimizerTerminateAction *terminateAction = new g2o::SparseOptimizerTerminateAction;
terminateAction->setGainThreshold(1e-10);
optimizer.addPostIterationAction(terminateAction);
optimizer.optimize(500);
for (int i = 0; i < position.size(); i++)
{
g2o::VertexPointXY *vertex = dynamic_cast<g2o::VertexPointXY *>(optimizer.vertex(i));
position[i](0) = vertex->estimate()(0);
position[i](1) = vertex->estimate()(1);
position[i](2) = 0;
}
LogWarn("Trajectory optimization successful !!!!!!!!!!");
}仿照这个来定义顶点和边，轨迹优化的观测信息是，上一个点和当前点的相对位姿，最后一个观测是最优一个点的真实位姿和误差位姿的相对变换，我原本这个代码是2维的轨迹优化，而你现在需要做的是3维的位置优化+姿态优化。然后，我希望你删除这个优化中的障碍物，不需要考虑障碍物，现在只需要做轨迹的位姿优化。
最新发布
本文详细介绍了如何在CMakeLists.txt文件中正确配置视觉里程计项目，通过添加必要的链接库，如g2o_types_slam3d，确保项目能够顺利编译运行。涉及的库包括slambase、OpenCV、PCL、CSPARSE、Sophus等。
扫一扫
1057
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
