搭建八叉树地图，降低点云数量，同时描述运动物体

#include < iostream>
#include < fstream>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <octomap/octomap.h> // for octomap

#include <Eigen/Geometry>
#include <boost/format.hpp> // for formating strings

int main(int argc, char **argv) {
vector< cv::Mat> colorImgs, depthImgs; // 彩色图和深度图
vector< Eigen::Isometry3d> poses; // 相机位姿
、、
ifstream fin("./data/pose.txt");
if (!fin) {
cerr << “cannot find pose file” << endl;
return 1;
}
、、
for (int i = 0; i < 5; i++) {
boost::format fmt("./data/%s/%d.%s"); //图像文件格式
colorImgs.push_back(cv::imread((fmt % “color” % (i + 1) % “png”).str()));
depthImgs.push_back(cv::imread((fmt % “depth” % (i + 1) % “png”).str(), -1)); // 使用-1读取原始图像
、、
double data[7] = {0};
for (int i = 0; i < 7; i++) {
fin >> data[i];
}
Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
Eigen::Isometry3d T(q);
T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
poses.push_back(T);
}
读取txt文件到Poses, 图片到colorImgs, depthImgs;
// 计算点云并拼接
// 相机内参
double cx = 319.5;
double cy = 239.5;
double fx = 481.2;
double fy = -480.0;
double depthScale = 5000.0;
、、
cout << “正在将图像转换为 Octomap …” << endl;
、、
// octomap tree
octomap::OcTree tree(0.01); // 初始化一个octomap tree,参数为分辨率，变量名为tree
、、
for (int i = 0; i < 5; i++) {
cout << "转换图像中: " << i + 1 << endl;
cv::Mat color = colorImgs[i];
cv::Mat depth = depthImgs[i];
Eigen::Isometry3d T = poses[i];
、、
octomap::Pointcloud cloud; // 初始化一个octomap下的点云，cloud
、、
for (int v = 0; v < color.rows; v++)
for (int u = 0; u < color.cols; u++) {
unsigned int d = depth.ptr(v)[u]; // 深度值
if (d == 0) continue; // 为0表示没有测量到
Eigen::Vector3d point;
point[2] = double(d) / depthScale;
point[0] = (u - cx) * point[2] / fx;
point[1] = (v - cy) * point[2] / fy;
Eigen::Vector3d pointWorld = T * point;
// 将世界坐标系的点放入cloud
cloud.push_back(pointWorld[0], pointWorld[1], pointWorld[2]);
}
//
// 将点云存入八叉树地图，给定原点，这样可以计算投射线
tree.insertPointCloud(cloud, octomap::point3d(T(0, 3), T(1, 3), T(2, 3)));
}
//
// 更新中间节点的占据信息并写入磁盘
tree.updateInnerOccupancy();
cout << "saving octomap … " << endl;
tree.writeBinary(“octomap.bt”); 保存为.bt文件
return 0;
}