362 lines
13 KiB
C++
362 lines
13 KiB
C++
|
/**
|
|||
|
|
* This file is part of ORB-SLAM2.
|
|||
|
|
*
|
|||
|
|
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
|
|||
|
|
* For more information see <https://github.com/raulmur/ORB_SLAM2>
|
|||
|
|
*
|
|||
|
|
* ORB-SLAM2 is free software: you can redistribute it and/or modify
|
|||
|
|
* it under the terms of the GNU General Public License as published by
|
|||
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|||
|
|
* (at your option) any later version.
|
|||
|
|
*
|
|||
|
|
* ORB-SLAM2 is distributed in the hope that it will be useful,
|
|||
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|||
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|||
|
|
* GNU General Public License for more details.
|
|||
|
|
*
|
|||
|
|
* You should have received a copy of the GNU General Public License
|
|||
|
|
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#include <iostream>
|
|||
|
|
#include <algorithm>
|
|||
|
|
#include <fstream>
|
|||
|
|
#include <iomanip>
|
|||
|
|
#include <chrono>
|
|||
|
|
#include <unistd.h>
|
|||
|
|
#include <opencv2/core/core.hpp>
|
|||
|
|
#include <vector>
|
|||
|
|
// #include <pcl/visualization/cloud_viewer.h>
|
|||
|
|
// #include <pcl/io/io.h>
|
|||
|
|
// #include <pcl/io/pcd_io.h>
|
|||
|
|
// #include <pcl/point_types.h>
|
|||
|
|
#include <opencv2/opencv.hpp>
|
|||
|
|
// #include <pcl/features/normal_3d.h>
|
|||
|
|
// #include <pcl/features/principal_curvatures.h>
|
|||
|
|
#include <System.h>
|
|||
|
|
|
|||
|
|
using namespace std;
|
|||
|
|
void LoadLabel(const string &strLabelFilename, vector<vector<double>> &vvLabel);
|
|||
|
|
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft,
|
|||
|
|
vector<string> &vstrImageRight, vector<string> &vstrlabel, vector<double> &vTimestamps);
|
|||
|
|
// pcl::PointCloud<pcl::PointXYZI>::Ptr BEV_GEN(const cv::String &strFile, cv::Mat &BEV_IMG);
|
|||
|
|
|
|||
|
|
int main(int argc, char **argv)
|
|||
|
|
{
|
|||
|
|
if (argc != 5)
|
|||
|
|
{
|
|||
|
|
cerr << endl
|
|||
|
|
<< "Usage: ./stereo_kitti path_to_vocabulary path_to_settings path_to_sequence path_to_cloud" << endl;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Retrieve paths to images
|
|||
|
|
vector<string> vstrImageLeft;
|
|||
|
|
vector<string> vstrImageRight;
|
|||
|
|
vector<double> vTimestamps;
|
|||
|
|
vector<string> vstrlabel;
|
|||
|
|
LoadImages(string(argv[3]), vstrImageLeft, vstrImageRight, vstrlabel, vTimestamps);
|
|||
|
|
cout << "读取成功" << endl;
|
|||
|
|
const int nImages = vstrImageLeft.size();
|
|||
|
|
|
|||
|
|
// Create SLAM system. It initializes all system threads and gets ready to process frames.
|
|||
|
|
ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::STEREO, true);
|
|||
|
|
|
|||
|
|
// Vector for tracking time statistics
|
|||
|
|
vector<float> vTimesTrack;
|
|||
|
|
vTimesTrack.resize(nImages);
|
|||
|
|
|
|||
|
|
cout << endl
|
|||
|
|
<< "-------" << endl;
|
|||
|
|
cout << "Start processing sequence ..." << endl;
|
|||
|
|
cout << "Images in the sequence: " << nImages << endl
|
|||
|
|
<< endl;
|
|||
|
|
|
|||
|
|
// Main loop
|
|||
|
|
cv::Mat imLeft, imRight;
|
|||
|
|
|
|||
|
|
cv::String pattern = string(argv[4]);
|
|||
|
|
std::vector<cv::String> fn;
|
|||
|
|
|
|||
|
|
glob(pattern, fn, false);
|
|||
|
|
|
|||
|
|
for (int ni = 0; ni < nImages; ni++)
|
|||
|
|
{
|
|||
|
|
cv::Mat imbev;
|
|||
|
|
// pcl::PointCloud<pcl::PointXYZI>::Ptr tmp; // 保存的原始点云数据，pcl格式
|
|||
|
|
// tmp = BEV_GEN(fn[ni], imbev); // imRGB为BEV视图
|
|||
|
|
|
|||
|
|
vector<vector<double>> vvLabel;
|
|||
|
|
// Read left and right images from file
|
|||
|
|
imLeft = cv::imread(vstrImageLeft[ni], CV_LOAD_IMAGE_UNCHANGED);
|
|||
|
|
imRight = cv::imread(vstrImageRight[ni], CV_LOAD_IMAGE_UNCHANGED);
|
|||
|
|
LoadLabel(vstrlabel[ni], vvLabel);
|
|||
|
|
|
|||
|
|
double tframe = vTimestamps[ni];
|
|||
|
|
if (imLeft.empty())
|
|||
|
|
{
|
|||
|
|
cerr << endl
|
|||
|
|
<< "Failed to load image at: "
|
|||
|
|
<< string(vstrImageLeft[ni]) << endl;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#ifdef COMPILEDWITHC11
|
|||
|
|
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
|
|||
|
|
#else
|
|||
|
|
std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
// Pass the images to the SLAM system
|
|||
|
|
double delta_t = 0.1;
|
|||
|
|
if (ni > 0)
|
|||
|
|
{
|
|||
|
|
delta_t = vTimestamps[ni] - vTimestamps[ni - 1];
|
|||
|
|
}
|
|||
|
|
SLAM.TrackStereo(imLeft, imRight, tframe, vvLabel, imbev, delta_t);
|
|||
|
|
#ifdef COMPILEDWITHC11
|
|||
|
|
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
|
|||
|
|
#else
|
|||
|
|
std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
double ttrack = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count();
|
|||
|
|
|
|||
|
|
vTimesTrack[ni] = ttrack;
|
|||
|
|
// Wait to load the next frame
|
|||
|
|
double T = 0;
|
|||
|
|
if (ni < nImages - 1)
|
|||
|
|
T = vTimestamps[ni + 1] - tframe;
|
|||
|
|
else if (ni > 0)
|
|||
|
|
T = tframe - vTimestamps[ni - 1];
|
|||
|
|
|
|||
|
|
if (ttrack < T)
|
|||
|
|
usleep((T - ttrack) * 1e6);
|
|||
|
|
}
|
|||
|
|
// Stop all threads
|
|||
|
|
SLAM.Shutdown();
|
|||
|
|
|
|||
|
|
// Tracking time statistics
|
|||
|
|
sort(vTimesTrack.begin(), vTimesTrack.end());
|
|||
|
|
float totaltime = 0;
|
|||
|
|
for (int ni = 0; ni < nImages; ni++)
|
|||
|
|
{
|
|||
|
|
totaltime += vTimesTrack[ni];
|
|||
|
|
}
|
|||
|
|
cout << "-------" << endl
|
|||
|
|
<< endl;
|
|||
|
|
cout << "median tracking time: " << vTimesTrack[nImages / 2] << endl;
|
|||
|
|
cout << "mean tracking time: " << totaltime / nImages << endl;
|
|||
|
|
|
|||
|
|
// Save camera trajectory
|
|||
|
|
SLAM.SaveTrajectoryKITTI("CameraTrajectory.txt");
|
|||
|
|
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
/*
|
|||
|
|
pcl::PointCloud<pcl::PointXYZI>::Ptr BEV_GEN(const cv::String &strFile, cv::Mat &BEV_IMG)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
int32_t num = 1000000;
|
|||
|
|
float *data = (float *)malloc(num * sizeof(float)); // void *malloc(size_t size) 分配所需的内存空间，并返回一个指向它的指针。
|
|||
|
|
|
|||
|
|
// pointers
|
|||
|
|
float *px = data + 0;
|
|||
|
|
float *py = data + 1;
|
|||
|
|
float *pz = data + 2;
|
|||
|
|
float *pr = data + 3;
|
|||
|
|
|
|||
|
|
// load point cloud
|
|||
|
|
pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZI>());
|
|||
|
|
FILE *stream;
|
|||
|
|
std::string Filename = strFile;
|
|||
|
|
char ch[200];
|
|||
|
|
strcpy(ch, Filename.c_str());
|
|||
|
|
|
|||
|
|
stream = fopen(ch, "rb");
|
|||
|
|
num = fread(data, sizeof(float), num, stream) / 4;
|
|||
|
|
point_cloud->width = num; // 设定长
|
|||
|
|
point_cloud->height = 1; // 设定高
|
|||
|
|
point_cloud->is_dense = false; // 如果没有无效点（例如，具有NaN或Inf值），则为True
|
|||
|
|
for (int32_t i = 0; i < num; i++)
|
|||
|
|
{
|
|||
|
|
// vector<int32_t> point_cloud;
|
|||
|
|
pcl::PointXYZI point;
|
|||
|
|
point.x = *px;
|
|||
|
|
point.y = *py;
|
|||
|
|
point.z = *pz;
|
|||
|
|
point.intensity = *pr;
|
|||
|
|
point_cloud->points.push_back(point);
|
|||
|
|
px += 4;
|
|||
|
|
py += 4;
|
|||
|
|
pz += 4;
|
|||
|
|
pr += 4;
|
|||
|
|
}
|
|||
|
|
fclose(stream);
|
|||
|
|
free(data); // 释放内存
|
|||
|
|
pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_new(new pcl::PointCloud<pcl::PointXYZI>());
|
|||
|
|
for (int i = 0; i < point_cloud->points.size(); i++)
|
|||
|
|
{
|
|||
|
|
if (point_cloud->points[i].x > 0 && point_cloud->points[i].x < 50 && point_cloud->points[i].y > -25 && point_cloud->points[i].y < 25 && point_cloud->points[i].z > -0.8 && point_cloud->points[i].z < 1.3)
|
|||
|
|
{
|
|||
|
|
point_cloud_new->push_back(point_cloud->points[i]);
|
|||
|
|
// cout<<point_cloud_out->points[j].x<<endl;
|
|||
|
|
// j++;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_Curvature(new pcl::PointCloud<pcl::PointXYZI>);
|
|||
|
|
pcl::copyPointCloud(*point_cloud_new, *cloud_Curvature); // src中的xyz覆盖掉src_PN中的xyz值，然后把xyz+normal的信息给src_PN
|
|||
|
|
// MaxCurvaturePoints(point_cloud_new, cloud_Curvature); //点云曲率
|
|||
|
|
// point_cloud_new->points.
|
|||
|
|
// 可视化
|
|||
|
|
|
|||
|
|
int Height = 608 + 1;
|
|||
|
|
int Width = (608 + 1);
|
|||
|
|
float Discretization = 50.0 / 608.0;
|
|||
|
|
|
|||
|
|
// pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_new_ = *point_cloud_new;
|
|||
|
|
|
|||
|
|
cv::Mat height_map = cv::Mat::zeros(608, 608, CV_8UC1);
|
|||
|
|
cv::Mat intensityMap = cv::Mat::zeros(608, 608, CV_8UC1);
|
|||
|
|
cv::Mat densityMap = cv::Mat::zeros(608, 608, CV_8UC1);
|
|||
|
|
cv::Mat CurvatureMap = cv::Mat::zeros(608, 608, CV_8UC1);
|
|||
|
|
|
|||
|
|
vector<vector<vector<cv::Point3f>>> PointCloud_frac(608, vector<vector<cv::Point3f>>(608, vector<cv::Point3f>()));
|
|||
|
|
|
|||
|
|
for (int i = 0; i < point_cloud_new->points.size(); i++)
|
|||
|
|
{
|
|||
|
|
point_cloud_new->points[i].y = (floor(point_cloud_new->points[i].y / Discretization) + Height / 2);
|
|||
|
|
point_cloud_new->points[i].x = (floor(point_cloud_new->points[i].x / Discretization));
|
|||
|
|
cloud_Curvature->points[i].y = (floor(cloud_Curvature->points[i].y / Discretization) + Height / 2);
|
|||
|
|
cloud_Curvature->points[i].x = (floor(cloud_Curvature->points[i].x / Discretization));
|
|||
|
|
// cout<<"int(point_cloud_new->points[i].x " <<int(point_cloud_new->points[i].x << "int(point_cloud_new->points[i].y " <<int(point_cloud_new->points[i].y <<endl;
|
|||
|
|
cv::Point3f p;
|
|||
|
|
p.x = point_cloud_new->points[i].z + 1.2;
|
|||
|
|
p.y = point_cloud_new->points[i].intensity;
|
|||
|
|
// p.z = cloud_Curvature->points[i].intensity; //曲率
|
|||
|
|
p.z = cloud_Curvature->points[i].intensity;
|
|||
|
|
PointCloud_frac[int(point_cloud_new->points[i].x)][int(point_cloud_new->points[i].y)].push_back(p);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// for(int i =0;i<608;i++)
|
|||
|
|
// {
|
|||
|
|
// for(int j =0;j<1218;j++)
|
|||
|
|
// // cout<<"poincloud _size " << PointCloud_frac[i][j].size()<<endl;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
for (int i = 0; i < 608; i++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < 608; j++)
|
|||
|
|
{
|
|||
|
|
if (PointCloud_frac[i][j].size() > 0)
|
|||
|
|
{
|
|||
|
|
vector<float> temp_h, temp_I, temp_c;
|
|||
|
|
for (int k = 0; k < PointCloud_frac[i][j].size(); k++)
|
|||
|
|
{
|
|||
|
|
temp_h.push_back(PointCloud_frac[i][j][k].x);
|
|||
|
|
temp_I.push_back(PointCloud_frac[i][j][k].y);
|
|||
|
|
temp_c.push_back(PointCloud_frac[i][j][k].z);
|
|||
|
|
}
|
|||
|
|
height_map.at<uchar>(i, j) = (int)(*max_element(temp_h.begin(), temp_h.end()) / 2.1 * 255.0);
|
|||
|
|
intensityMap.at<uchar>(i, j) = (int)(*max_element(temp_I.begin(), temp_I.end()) * 255.0);
|
|||
|
|
densityMap.at<uchar>(i, j) = (int)(min(1.0, log10(temp_h.size() + 1) / log10(64)) * 255.0);
|
|||
|
|
CurvatureMap.at<uchar>(i, j) = (int)min(255.0, (10000 * (*max_element(temp_c.begin(), temp_c.end())) * 255.0));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
vector<cv::Mat> vimg;
|
|||
|
|
vimg.push_back(intensityMap);
|
|||
|
|
vimg.push_back(height_map);
|
|||
|
|
vimg.push_back(densityMap);
|
|||
|
|
// vimg.push_back(intensityMap);
|
|||
|
|
// vimg.push_back(height_map);
|
|||
|
|
// vimg.push_back(CurvatureMap);
|
|||
|
|
|
|||
|
|
cv::merge(vimg, BEV_IMG);
|
|||
|
|
|
|||
|
|
cv::flip(BEV_IMG, BEV_IMG, 0);
|
|||
|
|
cv::flip(BEV_IMG, BEV_IMG, 1);
|
|||
|
|
return point_cloud;
|
|||
|
|
}
|
|||
|
|
*/
|
|||
|
|
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft,
|
|||
|
|
vector<string> &vstrImageRight, vector<string> &vstrlabel, vector<double> &vTimestamps)
|
|||
|
|
{
|
|||
|
|
ifstream fTimes;
|
|||
|
|
string strPathTimeFile = strPathToSequence + "/times.txt";
|
|||
|
|
fTimes.open(strPathTimeFile.c_str());
|
|||
|
|
while (!fTimes.eof())
|
|||
|
|
{
|
|||
|
|
string s;
|
|||
|
|
getline(fTimes, s);
|
|||
|
|
if (!s.empty())
|
|||
|
|
{
|
|||
|
|
stringstream ss;
|
|||
|
|
ss << s;
|
|||
|
|
double t;
|
|||
|
|
ss >> t;
|
|||
|
|
vTimestamps.push_back(t);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
cout << "读取times.txt完成" << endl;
|
|||
|
|
string strPrefixLeft = strPathToSequence + "/image_00/";
|
|||
|
|
string strPrefixRight = strPathToSequence + "/image_01/";
|
|||
|
|
string strlabel = strPathToSequence + "/labels/";
|
|||
|
|
const int nTimes = vTimestamps.size();
|
|||
|
|
vstrImageLeft.resize(nTimes);
|
|||
|
|
vstrImageRight.resize(nTimes);
|
|||
|
|
vstrlabel.resize(nTimes);
|
|||
|
|
for (int i = 0; i < nTimes; i++)
|
|||
|
|
{
|
|||
|
|
stringstream ss;
|
|||
|
|
ss << setfill('0') << setw(10) << i;
|
|||
|
|
vstrImageLeft[i] = strPrefixLeft + ss.str() + ".png";
|
|||
|
|
vstrImageRight[i] = strPrefixRight + ss.str() + ".png";
|
|||
|
|
// vstrlabel[i] = strlabel + ss.str() + ".txt";
|
|||
|
|
}
|
|||
|
|
for (int i = 0; i < nTimes; i++)
|
|||
|
|
{
|
|||
|
|
stringstream ss;
|
|||
|
|
ss << setfill('0') << setw(6) << i;
|
|||
|
|
|
|||
|
|
vstrlabel[i] = strlabel + ss.str() + ".txt";
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void LoadLabel(const string &strLabelFilename, vector<vector<double>> &vvLabel)
|
|||
|
|
{
|
|||
|
|
ifstream fAssociation;
|
|||
|
|
fAssociation.open(strLabelFilename.c_str());
|
|||
|
|
while (!fAssociation.eof())
|
|||
|
|
{
|
|||
|
|
string s;
|
|||
|
|
getline(fAssociation, s);
|
|||
|
|
if (!s.empty())
|
|||
|
|
{
|
|||
|
|
stringstream ss;
|
|||
|
|
ss << s;
|
|||
|
|
double cls, x, y, w, h, im, re, yaw;
|
|||
|
|
vector<double> label;
|
|||
|
|
ss >> cls;
|
|||
|
|
label.push_back(cls);
|
|||
|
|
ss >> x;
|
|||
|
|
x = 608 - x;
|
|||
|
|
label.push_back(x);
|
|||
|
|
ss >> y;
|
|||
|
|
y = 608 - y;
|
|||
|
|
label.push_back(y);
|
|||
|
|
ss >> w;
|
|||
|
|
label.push_back(w);
|
|||
|
|
ss >> h;
|
|||
|
|
label.push_back(h);
|
|||
|
|
ss >> im;
|
|||
|
|
label.push_back(im);
|
|||
|
|
ss >> re;
|
|||
|
|
label.push_back(re);
|
|||
|
|
ss >> yaw;
|
|||
|
|
label.push_back(yaw);
|
|||
|
|
vvLabel.push_back(label);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|