84 lines
2.1 KiB
C++
84 lines
2.1 KiB
C++
#ifndef MAP_BUILDER_H_
|
|
#define MAP_BUILDER_H_
|
|
|
|
#include "read_configs.h"
|
|
#include "camera.h"
|
|
#include "frame.h"
|
|
#include "correlation_flow.h"
|
|
#include "map.h"
|
|
#include "loop_closure.h"
|
|
#include "map_stitcher.h"
|
|
|
|
|
|
class MapBuilder{
|
|
public:
|
|
MapBuilder(Configs& configs);
|
|
|
|
bool AddNewInput(cv::Mat& image, double timestamp = -1);
|
|
void ComputeFFTResult(cv::Mat& image);
|
|
void ConstructFrame(double timestamp);
|
|
void SetCurrentFramePose();
|
|
bool Initialize();
|
|
void UpdateIntermedium();
|
|
void UpdateCurrentPose();
|
|
bool Tracking(Eigen::Vector3d& response);
|
|
void AddCFEdge();
|
|
void AddCFEdgeToMap(Eigen::Vector3d& relative_pose, int from, int to,
|
|
int edge_id, Edge::Type edge_type, Eigen::Matrix3d& info);
|
|
Eigen::Vector2d ComputeRelativeDA();
|
|
void SetFrameDistance();
|
|
bool FindLoopClosure();
|
|
void AddLoopEdges();
|
|
bool OptimizeMap();
|
|
void CheckAndOptimize();
|
|
void UpdateValueAfterLoop();
|
|
|
|
// for visualization
|
|
bool GetCFPose(Eigen::Vector3d& pose);
|
|
bool GetFramePoses(Aligned<std::vector, Eigen::Vector3d>& poses, std::vector<double>& timestamps);
|
|
bool GetOccupancyMapOrigin(
|
|
Eigen::Vector3d& pixel_origin, Eigen::Matrix<double, 7, 1>& real_origin); // [qw, qx, qy, qz, x, y, z]
|
|
double GetMapResolution();
|
|
OccupancyData& GetMapData();
|
|
|
|
private:
|
|
bool _init;
|
|
int _frame_id;
|
|
int _edge_id;
|
|
bool _last_lost;
|
|
|
|
// tmp
|
|
FramePtr _last_frame;
|
|
FramePtr _current_frame;
|
|
// image plane pose
|
|
Eigen::Vector3d _last_cf_pose;
|
|
Eigen::Vector3d _current_cf_pose;
|
|
// real scale pose
|
|
Eigen::Vector3d _last_cf_real_pose;
|
|
Eigen::Vector3d _current_cf_real_pose;
|
|
// robot pose
|
|
Eigen::Vector3d _last_pose;
|
|
Eigen::Vector3d _current_pose;
|
|
// distance
|
|
double _distance;
|
|
|
|
// intermedium rsults
|
|
Eigen::ArrayXXf _image_array;
|
|
Eigen::ArrayXXcf _last_fft_result;
|
|
Eigen::ArrayXXcf _last_fft_polar;
|
|
Eigen::ArrayXXcf _fft_result;
|
|
Eigen::ArrayXXcf _fft_polar;
|
|
std::vector<LoopClosureResult> _loop_matches;
|
|
|
|
Configs _configs;
|
|
CameraPtr _camera;
|
|
CorrelationFlowPtr _correlation_flow;
|
|
KeyframeSelectionConfig _kfs_config;
|
|
MapPtr _map;
|
|
LoopClosurePtr _loop_closure;
|
|
MapStitcherPtr _map_stitcher;
|
|
};
|
|
|
|
|
|
#endif // MAP_BUILDER_H
|