一、整体介绍

在图优化中，关键帧的作用是不言而喻的，一个关键帧就对应一个顶点。在这个系统中，关键中对应的文件有两类：

1. src/hdl_graph_slam/keyframe.cpp 和include/hdl_graph_slam/keyframe.hpp

它包含KeyFrame和KeyFrameSnapshot两个类。

KeyFrame类对应的所有优化相关的信息，包括gps、imu等信息都在里面，另外它还有一个作用就是把这些信息存入文件，并在需要的时候从文件中读取这些信息，其实是为了离线优化。

KeyFrameSnapshot类可以理解为简化版的KeyFrame类，它只包含位姿和点云信息，是拼接地图时用的，因为一张地图只需要每一帧的位姿和原始点云就好了呀

2. include/hdl_graph_slam/keyframe_updater.hpp

它包含KeyFramUpdater类，作用是更新关键帧，就是判断当前帧和上一个关键帧是否在距离或者角度上达到了一定差距，如果是，则把它设置为关键帧。

二、详细介绍

下面我们就进入这两个文件的内部，看一下到底做了啥

1. KeyFrame类

它对应的代码在文件keyframe.cpp/.hpp中

1）包含的信息

从keyframe.hpp的代码中，我们可以清晰看出一个关键帧都包含了哪些信息

ros::Time stamp;                                // timestamp
  Eigen::Isometry3d odom;                         // odometry (estimated by scan_matching_odometry)
  double accum_distance;                          // accumulated distance from the first node (by scan_matching_odometry)
  pcl::PointCloud<PointT>::ConstPtr cloud;        // point cloud
  boost::optional<Eigen::Vector4d> floor_coeffs;  // detected floor's coefficients
  boost::optional<Eigen::Vector3d> utm_coord;     // UTM coord obtained by GPS

  boost::optional<Eigen::Vector3d> acceleration;   //
  boost::optional<Eigen::Quaterniond> orientation; //

  g2o::VertexSE3* node;                           // node instance

英文注释应该也都能看得懂，不解释了吧

2）读写信息

读和写分别对应save和load函数，内容简单明了，直接贴代码吧

void KeyFrame::save(const std::string& directory) {
  if(!boost::filesystem::is_directory(directory)) {
    boost::filesystem::create_directory(directory);
  }

  std::ofstream ofs(directory + "/data");
  ofs << "stamp " << stamp.sec << " " << stamp.nsec << "\n";

  ofs << "estimate\n";
  ofs << node->estimate().matrix() << "\n";

  ofs << "odom\n";
  ofs << odom.matrix() << "\n";

  ofs << "accum_distance " << accum_distance << "\n";

  if(floor_coeffs) {
    ofs << "floor_coeffs " << floor_coeffs->transpose() << "\n";
  }

  if(utm_coord) {
      ofs << "utm_coord " << utm_coord->transpose() << "\n";
  }

  if(acceleration) {
      ofs << "acceleration " << acceleration->transpose() << "\n";
  }

  if(orientation) {
      ofs << "orientation " << orientation->w() << " " << orientation->x() << " " << orientation->y() << " " << orientation->z() << "\n";
  }

  if(node) {
    ofs << "id " << node->id() << "\n";
  }

  pcl::io::savePCDFileBinary(directory + "/cloud.pcd", *cloud);
}

bool KeyFrame::load(const std::string& directory, g2o::HyperGraph* graph) {
    std::ifstream ifs(directory + "/data");
    if(!ifs) {
        return false;
    }

    long node_id = -1;
    boost::optional<Eigen::Isometry3d> estimate;

    while(!ifs.eof()) {
        std::string token;
        ifs >> token;

        if(token == "stamp") {
            ifs >> stamp.sec >> stamp.nsec;
        } else if(token == "estimate") {
            Eigen::Matrix4d mat;
            for(int i=0; i<4; i++) {
                for(int j=0; j<4; j++) {
                    ifs >> mat(i, j);
                }
            }
            estimate = Eigen::Isometry3d::Identity();
            estimate->linear() = mat.block<3, 3>(0, 0);
            estimate->translation() = mat.block<3, 1>(0, 3);
        } else if(token == "odom") {
            Eigen::Matrix4d odom_mat = Eigen::Matrix4d::Identity();
            for(int i=0; i<4; i++) {
                for(int j=0; j<4; j++) {
                    ifs >> odom_mat(i, j);
                }
            }

            odom.setIdentity();
            odom.linear() = odom_mat.block<3, 3>(0, 0);
            odom.translation() = odom_mat.block<3, 1>(0, 3);
        } else if(token == "accum_distance") {
            ifs >> accum_distance;
        } else if(token == "floor_coeffs") {
            Eigen::Vector4d coeffs;
            ifs >> coeffs[0] >> coeffs[1] >> coeffs[2] >> coeffs[3];
            floor_coeffs = coeffs;
        } else if (token == "utm_coord") {
            Eigen::Vector3d coord;
            ifs >> coord[0] >> coord[1] >> coord[2];
            utm_coord = coord;
        } else if(token == "acceleration") {
            Eigen::Vector3d acc;
            ifs >> acc[0] >> acc[1] >> acc[2];
            acceleration = acc;
        } else if(token == "orientation") {
            Eigen::Quaterniond quat;
            ifs >> quat.w() >> quat.x() >> quat.y() >> quat.z();
            orientation = quat;
        } else if(token == "id") {
            ifs >> node_id;
        }
    }

    if(node_id < 0) {
        ROS_ERROR_STREAM("invalid node id!!");
        ROS_ERROR_STREAM(directory);
        return false;
    }

    node = dynamic_cast<g2o::VertexSE3*>(graph->vertices()[node_id]);
    if(node == nullptr) {
        ROS_ERROR_STREAM("failed to downcast!!");
        return false;
    }

    if(estimate) {
        node->setEstimate(*estimate);
    }

    pcl::PointCloud<PointT>::Ptr cloud_(new pcl::PointCloud<PointT>());
    pcl::io::loadPCDFile(directory + "/cloud.pcd", *cloud_);
    cloud = cloud_;

    return true;
}

2. KeyFrameSnapshot类

它对应的代码同样在文件keyframe.cpp/.hpp中。

包含的信息如下，可以看到只有位姿和点云

Eigen::Isometry3d pose;                   // pose estimated by graph optimization
pcl::PointCloud<PointT>::ConstPtr cloud;

构造函数如下

KeyFrameSnapshot::KeyFrameSnapshot(const Eigen::Isometry3d& pose, const pcl::PointCloud<PointT>::ConstPtr& cloud)
  : pose(pose),
    cloud(cloud)
{}

KeyFrameSnapshot::KeyFrameSnapshot(const KeyFrame::Ptr& key)
  : pose(key->node->estimate()),
    cloud(key->cloud)
{}

从它的构造函数中可以看出，在实际使用时，它的内容都是从KeyFrame类中获得的，具体什么时候获得，怎样获得，我们后面会讲

3. KeyFrameUpdater类

它在文件keyframe_updater.hpp中，它的作用和工作流程我们在上面已经介绍了，这些内容就放在一个update函数里，简单。

bool update(const Eigen::Isometry3d& pose) {
    // first frame is always registered to the graph
    if(is_first) {
      is_first = false;
      prev_keypose = pose;
      return true;
    }

    // calculate the delta transformation from the previous keyframe
    Eigen::Isometry3d delta = prev_keypose.inverse() * pose;
    double dx = delta.translation().norm();
    double da = std::acos(Eigen::Quaterniond(delta.linear()).w());

    // too close to the previous frame
    if(dx < keyframe_delta_trans && da < keyframe_delta_angle) {
      return false;
    }

    accum_distance += dx;
    prev_keypose = pose;
    return true;
  }

这个文件里包含的内容其实并不多，我认为把它独立成一个类的做法可有可无，会稍微清晰一些，但必要性也没有特别大。