#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>
#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/core/eigen_types.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace Eigen;
struct Edge {
size_t s; // Start index
size_t e; // End index
Isometry3d pose; // Relative pose between start and end
};
class INS {
public:
void optimizeTrajectory(std::vector<Isometry3d> &poses) {
std::vector<Edge> edgeData;
for (size_t i = 0; i < poses.size() - 1; ++i) {
Edge edge_data;
edge_data.s = i;
edge_data.e = i + 1;
edge_data.pose = poses[i].inverse() * poses[i + 1];
edgeData.push_back(edge_data);
}
// 添加最后一个点到第一个点的边
Edge edge_data;
edge_data.s = poses.size() - 1; // 最后一个点的索引
edge_data.e = 0; // 第一个点的索引
Isometry3d error_pose = poses[edge_data.s];
Isometry3d true_pose = error_pose * calibration_error_.Get();
LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
edge_data.pose = true_pose.inverse() * poses[edge_data.e];
edgeData.push_back(edge_data);
std::unique_ptr<g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>> linearSolver =
std::make_unique<g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>();
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(
std::make_unique<g2o::BlockSolverX>(std::move(linearSolver)));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
optimizer.setVerbose(false);
for (int i = 0; i < poses.size(); i++) {
g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId(i);
v->setEstimate(poses[i]);
if (i == 0) {
v->setFixed(true);
}
optimizer.addVertex(v);
}
for (const auto &pData : edgeData) {
g2o::EdgeSE3 *edge = new g2o::EdgeSE3();
edge->setVertex(0, optimizer.vertex(pData.s));
edge->setVertex(1, optimizer.vertex(pData.e));
if (&pData == &edgeData.back()) {
LogWarn("*******&pData == &edgeData.back()************");
edge->setInformation(Matrix6d::Identity() * 100000);
} else {
edge->setInformation(Matrix6d::Identity() * 0.001);
}
edge->setMeasurement(pData.pose);
optimizer.addEdge(edge);
}
optimizer.initializeOptimization();
optimizer.optimize(500);
for (int i = 0; i < poses.size(); i++) {
g2o::VertexSE3 *vertex = dynamic_cast<g2o::VertexSE3 *>(optimizer.vertex(i));
poses[i] = vertex->estimate();
}
LogWarn("Trajectory optimization successful !!!!!!!!!!");
}
private:
struct CalibrationError {
Isometry3d Get() const {
Isometry3d calib;
calib.setIdentity();
calib.translate(Vector3d(0.1, 0.1, 0.1)); // Example calibration error
calib.rotate(AngleAxisd(Deg2Rad(0.1), Vector3d::UnitZ())); // Example rotation error
return calib;
}
};
double Deg2Rad(double deg) const {
return deg * M_PI / 180.0;
}
CalibrationError calibration_error_;
template<typename T>
void LogInfo(const T& message) {
cout << "[INFO] " << message << endl;
}
template<typename T>
void LogWarn(const T& message) {
cout << "[WARN] " << message << endl;
}
string matrixToString(const Matrix4d& matrix) {
stringstream ss;
ss << matrix;
return ss.str();
}
};
void generateSimulatedTrajectory(std::vector<Isometry3d>& poses, int numPoints) {
double sideLength = 4.0; // 正方形的边长
double angleStep = INS().Deg2Rad(90.0); // 每个转角的角度
for (int i = 0; i < numPoints; ++i) {
Isometry3d pose = Isometry3d::Identity();
switch (i % 4) {
case 0: pose.translation() = Vector3d(sideLength * (i / 4), 0, 0); break;
case 1: pose.translation() = Vector3d(sideLength * (i / 4 + 1), 0, 0);
pose.rotate(AngleAxisd(angleStep, Vector3d::UnitZ())); break;
case 2: pose.translation() = Vector3d(sideLength * (i / 4 + 1), sideLength, 0);
pose.rotate(AngleAxisd(angleStep * 2, Vector3d::UnitZ())); break;
case 3: pose.translation() = Vector3d(sideLength * (i / 4), sideLength, 0);
pose.rotate(AngleAxisd(angleStep * 3, Vector3d::UnitZ())); break;
}
// 加入误差
pose.translate(Vector3d(rand() / double(RAND_MAX) * 0.1, rand() / double(RAND_MAX) * 0.1, rand() / double(RAND_MAX) * 0.1));
pose.rotate(AngleAxisd(INS().Deg2Rad(rand() % 2), Vector3d::UnitZ()));
poses.push_back(pose);
}
}
void drawTrajectory(const std::vector<Isometry3d>& poses, const std::string& title) {
cv::Mat img(800, 800, CV_8UC3, cv::Scalar(255, 255, 255));
for (size_t i = 0; i < poses.size(); ++i) {
Vector3d pos = poses[i].translation();
if (i > 0) {
Vector3d prevPos = poses[i-1].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() {
srand(time(NULL));
int numTrajectoryPoints = 16; // 轨迹点数量(正方形的四个角重复四次)
std::vector<Isometry3d> trajectory(numTrajectoryPoints);
// 生成模拟轨迹
generateSimulatedTrajectory(trajectory, numTrajectoryPoints);
// 显示优化前的轨迹
drawTrajectory(trajectory, "Before Optimization");
// 创建INS对象并进行优化
INS ins_optimizer;
ins_optimizer.optimizeTrajectory(trajectory);
// 显示优化后的轨迹
drawTrajectory(trajectory, "After Optimization");
cv::waitKey(0);
return 0;
}
报错:/usr/include/c++/9/ostream:523:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<char, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /usr/include/c++/9/iostream:39,
from /home/guo/g2o仿真/src/optimize.cc:1:
/usr/include/c++/9/ostream:528:5: note: candidate: ‘template<class _Traits> std::basic_ostream<char, _Traits>& std::operator<<(std::basic_ostream<char, _Traits>&, unsigned char)’
528 | operator<<(basic_ostream<char, _Traits>& __out, unsigned char __c)
| ^~~~~~~~
/usr/include/c++/9/ostream:528:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<char, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /usr/include/c++/9/iostream:39,
from /home/guo/g2o仿真/src/optimize.cc:1:
/usr/include/c++/9/ostream:548:5: note: candidate: ‘template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, const _CharT*)’
548 | operator<<(basic_ostream<_CharT, _Traits>& __out, const _CharT* __s)
| ^~~~~~~~
/usr/include/c++/9/ostream:548:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /usr/include/c++/9/ostream:702,
from /usr/include/c++/9/iostream:39,
from /home/guo/g2o仿真/src/optimize.cc:1:
/usr/include/c++/9/bits/ostream.tcc:321:5: note: candidate: ‘template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, const char*)’
321 | operator<<(basic_ostream<_CharT, _Traits>& __out, const char* __s)
| ^~~~~~~~
/usr/include/c++/9/bits/ostream.tcc:321:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /usr/include/c++/9/iostream:39,
from /home/guo/g2o仿真/src/optimize.cc:1:
/usr/include/c++/9/ostream:565:5: note: candidate: ‘template<class _Traits> std::basic_ostream<char, _Traits>& std::operator<<(std::basic_ostream<char, _Traits>&, const char*)’
565 | operator<<(basic_ostream<char, _Traits>& __out, const char* __s)
| ^~~~~~~~
/usr/include/c++/9/ostream:565:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<char, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /usr/include/c++/9/iostream:39,
from /home/guo/g2o仿真/src/optimize.cc:1:
/usr/include/c++/9/ostream:578:5: note: candidate: ‘template<class _Traits> std::basic_ostream<char, _Traits>& std::operator<<(std::basic_ostream<char, _Traits>&, const signed char*)’
578 | operator<<(basic_ostream<char, _Traits>& __out, const signed char* __s)
| ^~~~~~~~
/usr/include/c++/9/ostream:578:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<char, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /usr/include/c++/9/iostream:39,
from /home/guo/g2o仿真/src/optimize.cc:1:
/usr/include/c++/9/ostream:583:5: note: candidate: ‘template<class _Traits> std::basic_ostream<char, _Traits>& std::operator<<(std::basic_ostream<char, _Traits>&, const unsigned char*)’
583 | operator<<(basic_ostream<char, _Traits>& __out, const unsigned char* __s)
| ^~~~~~~~
/usr/include/c++/9/ostream:583:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<char, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /usr/include/c++/9/iostream:39,
from /home/guo/g2o仿真/src/optimize.cc:1:
/usr/include/c++/9/ostream:691:5: note: candidate: ‘template<class _Ostream, class _Tp> typename std::enable_if<std::__and_<std::__not_<std::is_lvalue_reference<_Tp> >, std::__is_convertible_to_basic_ostream<_Ostream>, std::__is_insertable<typename std::__is_convertible_to_basic_ostream<_Tp>::__ostream_type, const _Tp&, void> >::value, typename std::__is_convertible_to_basic_ostream<_Tp>::__ostream_type>::type std::operator<<(_Ostream&&, const _Tp&)’
691 | operator<<(_Ostream&& __os, const _Tp& __x)
| ^~~~~~~~
/usr/include/c++/9/ostream:691:5: note: template argument deduction/substitution failed:
/usr/include/c++/9/ostream: In substitution of ‘template<class _Ostream, class _Tp> typename std::enable_if<std::__and_<std::__not_<std::is_lvalue_reference<_Tp> >, std::__is_convertible_to_basic_ostream<_Ostream>, std::__is_insertable<typename std::__is_convertible_to_basic_ostream<_Tp>::__ostream_type, const _Tp&, void> >::value, typename std::__is_convertible_to_basic_ostream<_Tp>::__ostream_type>::type std::operator<<(_Ostream&&, const _Tp&) [with _Ostream = const char (&)[32]; _Tp = std::__cxx11::basic_string<char>]’:
/home/guo/g2o仿真/src/optimize.cc:46:103: required from here
/usr/include/c++/9/ostream:691:5: error: no type named ‘type’ in ‘struct std::enable_if<false, void>’
In file included from /usr/local/include/eigen3/Eigen/Core:50,
from /home/guo/g2o仿真/src/optimize.cc:4:
/usr/include/c++/9/complex:552:5: note: candidate: ‘template<class _Tp, class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, const std::complex<_Tp>&)’
552 | operator<<(basic_ostream<_CharT, _Traits>& __os, const complex<_Tp>& __x)
| ^~~~~~~~
/usr/include/c++/9/complex:552:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /opt/ros/noetic/include/g2o/core/hyper_graph.h:31,
from /opt/ros/noetic/include/g2o/core/optimizable_graph.h:35,
from /opt/ros/noetic/include/g2o/core/base_vertex.h:30,
from /home/guo/g2o仿真/src/optimize.cc:6:
/usr/include/c++/9/bitset:1538:5: note: candidate: ‘template<class _CharT, class _Traits, long unsigned int _Nb> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, const std::bitset<_Nb>&)’
1538 | operator<<(std::basic_ostream<_CharT, _Traits>& __os,
| ^~~~~~~~
/usr/include/c++/9/bitset:1538:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /usr/include/c++/9/memory:81,
from /opt/ros/noetic/include/g2o/core/robust_kernel.h:30,
from /opt/ros/noetic/include/g2o/core/base_binary_edge.h:34,
from /home/guo/g2o仿真/src/optimize.cc:7:
/usr/include/c++/9/bits/shared_ptr.h:66:5: note: candidate: ‘template<class _Ch, class _Tr, class _Tp, __gnu_cxx::_Lock_policy _Lp> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, const std::__shared_ptr<_Tp, _Lp>&)’
66 | operator<<(std::basic_ostream<_Ch, _Tr>& __os,
| ^~~~~~~~
/usr/include/c++/9/bits/shared_ptr.h:66:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /opt/ros/noetic/include/g2o/core/sparse_block_matrix.h:34,
from /opt/ros/noetic/include/g2o/core/solver.h:31,
from /opt/ros/noetic/include/g2o/core/block_solver.h:31,
from /home/guo/g2o仿真/src/optimize.cc:9:
/usr/include/c++/9/iomanip:79:5: note: candidate: ‘template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, std::_Resetiosflags)’
79 | operator<<(basic_ostream<_CharT, _Traits>& __os, _Resetiosflags __f)
| ^~~~~~~~
/usr/include/c++/9/iomanip:79:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /opt/ros/noetic/include/g2o/core/sparse_block_matrix.h:34,
from /opt/ros/noetic/include/g2o/core/solver.h:31,
from /opt/ros/noetic/include/g2o/core/block_solver.h:31,
from /home/guo/g2o仿真/src/optimize.cc:9:
/usr/include/c++/9/iomanip:109:5: note: candidate: ‘template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, std::_Setiosflags)’
109 | operator<<(basic_ostream<_CharT, _Traits>& __os, _Setiosflags __f)
| ^~~~~~~~
/usr/include/c++/9/iomanip:109:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /opt/ros/noetic/include/g2o/core/sparse_block_matrix.h:34,
from /opt/ros/noetic/include/g2o/core/solver.h:31,
from /opt/ros/noetic/include/g2o/core/block_solver.h:31,
from /home/guo/g2o仿真/src/optimize.cc:9:
/usr/include/c++/9/iomanip:143:5: note: candidate: ‘template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, std::_Setbase)’
143 | operator<<(basic_ostream<_CharT, _Traits>& __os, _Setbase __f)
| ^~~~~~~~
/usr/include/c++/9/iomanip:143:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /opt/ros/noetic/include/g2o/core/sparse_block_matrix.h:34,
from /opt/ros/noetic/include/g2o/core/solver.h:31,
from /opt/ros/noetic/include/g2o/core/block_solver.h:31,
from /home/guo/g2o仿真/src/optimize.cc:9:
/usr/include/c++/9/iomanip:178:5: note: candidate: ‘template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, std::_Setfill<_CharT>)’
178 | operator<<(basic_ostream<_CharT, _Traits>& __os, _Setfill<_CharT> __f)
| ^~~~~~~~
/usr/include/c++/9/iomanip:178:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /opt/ros/noetic/include/g2o/core/sparse_block_matrix.h:34,
from /opt/ros/noetic/include/g2o/core/solver.h:31,
from /opt/ros/noetic/include/g2o/core/block_solver.h:31,
from /home/guo/g2o仿真/src/optimize.cc:9:
/usr/include/c++/9/iomanip:208:5: note: candidate: ‘template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, std::_Setprecision)’
208 | operator<<(basic_ostream<_CharT, _Traits>& __os, _Setprecision __f)
| ^~~~~~~~
/usr/include/c++/9/iomanip:208:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /opt/ros/noetic/include/g2o/core/sparse_block_matrix.h:34,
from /opt/ros/noetic/include/g2o/core/solver.h:31,
from /opt/ros/noetic/include/g2o/core/block_solver.h:31,
from /home/guo/g2o仿真/src/optimize.cc:9:
/usr/include/c++/9/iomanip:238:5: note: candidate: ‘template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, std::_Setw)’
238 | operator<<(basic_ostream<_CharT, _Traits>& __os, _Setw __f)
| ^~~~~~~~
/usr/include/c++/9/iomanip:238:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /opt/ros/noetic/include/g2o/core/sparse_block_matrix.h:34,
from /opt/ros/noetic/include/g2o/core/solver.h:31,
from /opt/ros/noetic/include/g2o/core/block_solver.h:31,
from /home/guo/g2o仿真/src/optimize.cc:9:
/usr/include/c++/9/iomanip:311:5: note: candidate: ‘template<class _CharT, class _Traits, class _MoneyT> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, std::_Put_money<_MoneyT>)’
311 | operator<<(basic_ostream<_CharT, _Traits>& __os, _Put_money<_MoneyT> __f)
| ^~~~~~~~
/usr/include/c++/9/iomanip:311:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /opt/ros/noetic/include/g2o/core/sparse_block_matrix.h:34,
from /opt/ros/noetic/include/g2o/core/solver.h:31,
from /opt/ros/noetic/include/g2o/core/block_solver.h:31,
from /home/guo/g2o仿真/src/optimize.cc:9:
/usr/include/c++/9/iomanip:363:5: note: candidate: ‘template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, std::_Put_time<_CharT>)’
363 | operator<<(basic_ostream<_CharT, _Traits>& __os, _Put_time<_CharT> __f)
| ^~~~~~~~
/usr/include/c++/9/iomanip:363:5: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: mismatched types ‘std::basic_ostream<_CharT, _Traits>’ and ‘const char [32]’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
In file included from /usr/local/include/eigen3/Eigen/Core:269,
from /home/guo/g2o仿真/src/optimize.cc:4:
/usr/local/include/eigen3/Eigen/src/Core/IO.h:249:16: note: candidate: ‘template<class Derived> std::ostream& Eigen::operator<<(std::ostream&, const Eigen::DenseBase<Derived>&)’
249 | std::ostream & operator <<
| ^~~~~~~~
/usr/local/include/eigen3/Eigen/src/Core/IO.h:249:16: note: template argument deduction/substitution failed:
/home/guo/g2o仿真/src/optimize.cc:46:103: note: ‘std::string’ {aka ‘std::__cxx11::basic_string<char>’} is not derived from ‘const Eigen::DenseBase<Derived>’
46 | LogInfo("The map optimization error is:\n" << matrixToString(calibration_error_.Get().matrix()));
| ^
/home/guo/g2o仿真/src/optimize.cc:77:38: error: ‘Matrix6d’ has not been declared
77 | edge->setInformation(Matrix6d::Identity() * 100000);
| ^~~~~~~~
/home/guo/g2o仿真/src/optimize.cc:79:38: error: ‘Matrix6d’ has not been declared
79 | edge->setInformation(Matrix6d::Identity() * 0.001);
| ^~~~~~~~
/home/guo/g2o仿真/src/optimize.cc: In member function ‘Eigen::Isometry3d INS::CalibrationError::Get() const’:
/home/guo/g2o仿真/src/optimize.cc:101:48: error: cannot call member function ‘double INS::Deg2Rad(double) const’ without object
101 | calib.rotate(AngleAxisd(Deg2Rad(0.1), Vector3d::UnitZ())); // Example rotation error
| ^
/home/guo/g2o仿真/src/optimize.cc: In function ‘void generateSimulatedTrajectory(std::vector<Eigen::Transform<double, 3, 1> >&, int)’:
/home/guo/g2o仿真/src/optimize.cc:131:42: error: ‘double INS::Deg2Rad(double) const’ is private within this context
131 | double angleStep = INS().Deg2Rad(90.0); // 每个转角的角度
| ^
/home/guo/g2o仿真/src/optimize.cc:106:12: note: declared private here
106 | double Deg2Rad(double deg) const {
| ^~~~~~~
/home/guo/g2o仿真/src/optimize.cc:146:56: error: ‘double INS::Deg2Rad(double) const’ is private within this context
146 | pose.rotate(AngleAxisd(INS().Deg2Rad(rand() % 2), Vector3d::UnitZ()));
| ^
/home/guo/g2o仿真/src/optimize.cc:106:12: note: declared private here
106 | double Deg2Rad(double deg) const {
| ^~~~~~~
make[2]: *** [CMakeFiles/g2o_demo.dir/build.make:76:CMakeFiles/g2o_demo.dir/optimize.cc.o] 错误 1
make[1]: *** [CMakeFiles/Makefile2:673:CMakeFiles/g2o_demo.dir/all] 错误 2
make: *** [Makefile:146:all] 错误 2 给出完整修改后代码
最新发布