aruco板_基于arucoTag的简单slam

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include "g2o/types/slam3d/types_slam3d.h"

#include "g2o/types/slam3d/vertex_pointxyz.h"

#include "g2o/types/slam3d/vertex_se3.h"

#include "g2o/types/slam3d/edge_se3.h"

#include "g2o/types/slam3d/edge_se3_pointxyz.h"

#include "g2o/core/factory.h"

#include "g2o/core/optimization_algorithm_factory.h"

#include "g2o/core/sparse_optimizer.h"

#include "g2o/core/block_solver.h"

#include "g2o/solvers/dense/linear_solver_dense.h"

#include "g2o/core/optimization_algorithm_levenberg.h"

#include "icp_g2o.hpp"

using namespace std;

using namespace cv;

using namespace Eigen;

using namespace g2o;

//使用图片

//输入当前帧检测到的tag

//维护一张节点地图用于g2o

class ArucoFrame

{

public:

vector landmark_id;

vector<:vector3d> landmarks_pointXYZ;//在相机坐标系下的位置

Eigen::Isometry3d robot_pose;//在map坐标系下的位姿

};

class ArucoMap

{

public:

vector landmark_id;

vector<:vector3d> landmarks_pointXYZ;//在Map坐标系下的位置

};

ArucoMap ArucoMapGlobal;

vector ArucoFrameGlobal;

int id_vertex_pose=0, id_vertex_pointXYZ_start=100;//机器人pose的初始点认为是建图坐标系原点，id从0开始，tag的节点id从100开始。默认机器人pose不可能超过100个

void buildMap(vector< int > &ids, vector< vector< Point2f > > &corners, vector< Vec3d > &rvecs, vector< Vec3d > &tvecs, g2o::SparseOptimizer &optimizer)

{

//获取当前帧里的tag

ArucoFrame ArucoFrame_temp;

if(ids.size() != 0)

{

ArucoFrame_temp.robot_pose = Eigen::Isometry3d::Identity();

for(int i=0; i

{

ArucoFrame_temp.landmark_id.push_back(ids[i]);

//cv转换到eigen下

Mat rot_mat;

Rodrigues(rvecs[i], rot_mat);

Eigen::Matrix3d eigen_r;

Eigen::Vector3d eigen_t;

cv2eigen(rot_mat, eigen_r);

cv2eigen(tvecs[i],eigen_t);

//这里的rt对应的T是从mark坐标系变到相机坐标系

//Point_camera = T_camera-mark * Point_mark

//很重要！！所以这里的四个点是按mark坐标系下的顺序排列的

ArucoFrame_temp.landmarks_pointXYZ.push_back( eigen_r * Eigen::Vector3d(-0.03,0.03,0)+eigen_t );

ArucoFrame_temp.landmarks_pointXYZ.push_back( eigen_r * Eigen::Vector3d(0.03,0.03,0)+eigen_t );

ArucoFrame_temp.landmarks_pointXYZ.push_back( eigen_r * Eigen::Vector3d(0.03,-0.03,0)+eigen_t );

ArucoFrame_temp.landmarks_pointXYZ.push_back( eigen_r * Eigen::Vector3d(-0.03,-0.03,0)+eigen_t );

}

ArucoFrameGlobal.push_back(ArucoFrame_temp);

}

else

return ;

//对地图操作

if(ArucoMapGlobal.landmark_id.size() == 0)//初始化第一帧

{

for(int i=0; i

{

ArucoMapGlobal.landmark_id.push_back(ArucoFrame_temp.landmark_id[i]);

ArucoMapGlobal.landmarks_pointXYZ.push_back(ArucoFrame_temp.landmarks_pointXYZ[i*4+0]);

ArucoMapGlobal.landmarks_pointXYZ.push_back(ArucoFrame_temp.landmarks_pointXYZ[i*4+1]);

ArucoMapGlobal.landmarks_pointXYZ.push_back(ArucoFrame_temp.landmarks_pointXYZ[i*4+2]);

ArucoMapGlobal.landmarks_pointXYZ.push_back(ArucoFrame_temp.landmarks_pointXYZ[i*4+3]);

//添加pose

VertexSE3* v = new VertexSE3;

v->setEstimate(ArucoFrame_temp.robot_pose);

v->setId(id_vertex_pose++);

optimizer.addVertex(v);

/