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);
|
||
}
|
||
}
|
||
}
|