swh:1:snp:67458089460e0f4324e7769efad93379abfcf654
Tip revision: 8cf122b9c7f7403efe90dca3b0af0ebfec10b26d authored by jzhzhang on 07 January 2024, 04:10:10 UTC
Update pose_estimation.cpp
Update pose_estimation.cpp
Tip revision: 8cf122b
main.cpp
#include <rosefusion.h>
#include <DataReader.h>
#include <iostream>
#include <string>
#include <opencv2/opencv.hpp>
#include <ctime>
#include <fstream>
#include <pangolin/pangolin.h>
int main(int argc,char* argv[]){
std::cout<<"Read configure file\n";
const std::string camera_file(argv[1]);
const std::string data_file(argv[2]);
const std::string controller_file(argv[3]);
std::cout<<"Init configuration\n";
printf("%s\n",camera_file.c_str());
printf("%s\n",data_file.c_str());
printf("%s\n",controller_file.c_str());
const rosefusion::CameraParameters camera_config(camera_file);
const rosefusion::DataConfiguration data_config(data_file);
const rosefusion::ControllerConfiguration controller_config(controller_file);
pangolin::View color_cam;
pangolin::View shaded_cam;
pangolin::View depth_cam;
pangolin::GlTexture imageTexture;
pangolin::GlTexture shadTexture;
pangolin::GlTexture depthTexture;
if (controller_config.render_surface){
pangolin::CreateWindowAndBind("Main",2880,1440);
color_cam = pangolin::Display("color_cam")
.SetAspect((float)camera_config.image_width/(float)camera_config.image_height);
shaded_cam = pangolin::Display("shaded_cam")
.SetAspect((float)camera_config.image_width/(float)camera_config.image_height);
depth_cam = pangolin::Display("depth_cam")
.SetAspect((float)camera_config.image_width/(float)camera_config.image_height);
pangolin::Display("window")
.SetBounds(0.0, 1.0, 0.0, 1.0 )
.SetLayout(pangolin::LayoutEqual)
.AddDisplay(shaded_cam)
.AddDisplay(color_cam)
.AddDisplay(depth_cam);
imageTexture=pangolin::GlTexture(camera_config.image_width,camera_config.image_height,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE);
shadTexture=pangolin::GlTexture(camera_config.image_width,camera_config.image_height,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE);
depthTexture=pangolin::GlTexture(camera_config.image_width,camera_config.image_height,GL_LUMINANCE,false,0,GL_LUMINANCE,GL_UNSIGNED_BYTE);
}
cv::Mat shaded_img(camera_config.image_height, camera_config.image_width,CV_8UC3);
rosefusion::Pipeline pipeline { camera_config, data_config, controller_config };
clock_t time_stt=clock( );
cv::Mat color_img;
cv::Mat depth_map;
int n_imgs=0;
std::cout<<"Read seq file: "<<data_config.seq_file<<"\n";
DataReader d_reader(data_config.seq_file,false);
while( d_reader.hasMore()){
printf("n:%d\n",n_imgs);
d_reader.getNextFrame(color_img,depth_map);
bool success = pipeline.process_frame(depth_map, color_img,shaded_img);
if (!success){
std::cout << "Frame could not be processed" << std::endl;
}
if (controller_config.render_surface){
glClear(GL_COLOR_BUFFER_BIT);
color_cam.Activate();
imageTexture.Upload(color_img.data,GL_BGR,GL_UNSIGNED_BYTE);
imageTexture.RenderToViewportFlipY();
depth_cam.Activate();
depth_map.convertTo(depth_map,CV_8U,256/5000.0);
depthTexture.Upload(depth_map.data,GL_LUMINANCE,GL_UNSIGNED_BYTE);
depthTexture.RenderToViewportFlipY();
if (success){
shaded_cam.Activate();
shadTexture.Upload(shaded_img.data,GL_BGR,GL_UNSIGNED_BYTE);
shadTexture.RenderToViewportFlipY();
}
pangolin::FinishFrame();
}
n_imgs++;
}
std::cout <<"time per frame="<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC/n_imgs<<"ms"<<std::endl;
if (controller_config.save_trajectory){
pipeline.get_poses();
}
if (controller_config.save_scene){
auto points = pipeline.extract_pointcloud();
rosefusion::export_ply(data_config.result_path+data_config.seq_name+"_points.ply",points);
}
return 0;
}