基于arucoTag的简单slam

这只是一次简单的尝试，不过收获还是挺多的。
首先是地面上有一些tag，用摄像头去拍摄，每次只拍到部分的tag，拍多次。
因为tag是唯一的，而且尺度已知，可以直接计算摄像头的RT，因此可以套用slam框架。基本思路如下：
1.初始帧将map坐标系远点设为摄像头初始位置，将初始帧看到的tag的四个角点放入map中。
2.之后的帧先识别当前帧里的tag，分为两类，一类是map中已有的，一类是map中没有的。已有的那部分与map中的tag做匹配，使用icp方法求解出当前摄像头位姿，再将没有的那部分根据相机位姿再拼入map中。
3.最后做一次g2o优化。
g2o 顶点是相机pose （VertexSE3） tag的四个顶点landmark（VertexPointXYZ）
边是 tag的测量值（EdgeSE3PointXYZ）
其中采集图像要注意，按时间顺序，新的图像中的tag必须有map中已有的。

结果并不让人满意，但至少值得分析。
g2o的优化结果可以看出摄像头不在一个平面上，但实际上拍摄时摄像头都是摆在地上的。这主要是因为tag的z轴测的不准，导致摄像头有很剧烈的上下变化。
对于tag的位置优化结果，其实也很差，我是摆成长方形的，但是很明显优化出来的各个点不在长方形上。
从这个结果，我很跳跃的想到，在摄像头标定时，摄像头最好让视野中绝大部分有标定板存在，这样可以避免像素级带来的误差。我这个试验中，tag只占视野里很少的部分，所以误差特别大。

代码有些乱，仅作记录。

#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <ctime>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <Eigen/Geometry>

#include <opencv2/highgui.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d.hpp>

#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<int> landmark_id;
    vector<Eigen::Vector3d> landmarks_pointXYZ;//在相机坐标系下的位置
    Eigen::Isometry3d robot_pose;//在map坐标系下的位姿
};

class ArucoMap
{
public:
    vector<int> landmark_id;
    vector<Eigen::Vector3d> landmarks_pointXYZ;//在Map坐标系下的位置
};
ArucoMap ArucoMapGlobal;
vector<ArucoFrame> 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<ids.size(); 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 );