Skip to content

Commit

Permalink
In progress. modified half way through the commit process.
Browse files Browse the repository at this point in the history
  • Loading branch information
xuhao3e8 committed Dec 12, 2024
1 parent d411145 commit 4c0e48f
Show file tree
Hide file tree
Showing 14 changed files with 170 additions and 151 deletions.
15 changes: 10 additions & 5 deletions d2common/include/d2common/d2state.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,14 +2,15 @@
#include <set>
#include <map>
#include <d2common/d2vinsframe.h>
#include <spdlog/spdlog.h>

namespace D2Common {
class D2State {
protected:
int self_id;
std::set<int> all_drones;
int reference_frame_id = -1;
std::map<FrameIdType, D2BaseFrame*> frame_db;
std::map<FrameIdType, std::shared_ptr<D2BaseFrame>> frame_db;
std::map<FrameIdType, StatePtr> _frame_pose_state;

//This returns the perturb of the frame. per = [T, v], where v is the rotation vector representation of a small R.
Expand Down Expand Up @@ -44,18 +45,22 @@ class D2State {
return frame_db.find(frame_id) != frame_db.end();
}

const D2BaseFrame * getFramebyId(int frame_id) const {
const std::shared_ptr<D2BaseFrame> getFramebyId(int frame_id) const {
const Guard lock(state_lock);
if (frame_db.find(frame_id) == frame_db.end()) {
printf("\033[0;31m[D2EstimatorState::getFramebyId] Frame %d not found in database\033[0m\n", frame_id);
SPDLOG_ERROR("Frame {} not found in database", frame_id);
assert(true && "Frame not found in database");
return nullptr;
}
return frame_db.at(frame_id);
}

D2BaseFrame * getFramebyId(int frame_id) {
std::shared_ptr<D2BaseFrame> getFramebyId(int frame_id) {
const Guard lock(state_lock);
if (frame_db.find(frame_id) == frame_db.end()) {
printf("\033[0;31m[D2EstimatorState::getFramebyId] Frame %d not found in database\033[0m\n", frame_id);
SPDLOG_ERROR("Frame {} not found in database", frame_id);
assert(true && "Frame not found in database");
return nullptr;
}
return frame_db.at(frame_id);
}
Expand Down
3 changes: 2 additions & 1 deletion d2common/include/d2common/d2vinsframe.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,5 +34,6 @@ struct VINSFrame: public D2BaseFrame {
return D2BaseFrame(stamp, frame_id, drone_id, reference_frame_id, is_keyframe, odom, initial_ego_pose);
}
};


using VINSFramePtr = std::shared_ptr<VINSFrame>;
}
8 changes: 7 additions & 1 deletion d2frontend/include/d2frontend/d2featuretracker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,17 @@ using namespace Eigen;
#define LKImageInfo LKImageInfoCPU
#endif

namespace D2Common {
struct VINSFrame;
using VINSFramePtr = std::shared_ptr<VINSFrame>;
}

namespace D2FrontEnd {
using D2Common::VisualImageDescArray;
using D2Common::VisualImageDesc;
using D2Common::LandmarkIdType;
using D2Common::FrameIdType;
using D2Common::VINSFramePtr;

struct D2FTConfig {
bool show_feature_id = true;
Expand Down Expand Up @@ -156,7 +162,7 @@ class D2FeatureTracker {
D2FeatureTracker(D2FTConfig config);
bool trackLocalFrames(VisualImageDescArray & frames);
bool trackRemoteFrames(VisualImageDescArray & frames);
void updatebySldWin(const std::vector<VINSFrame*> sld_win);
void updatebySldWin(const std::vector<VINSFramePtr> sld_win);
void updatebyLandmarkDB(const std::map<LandmarkIdType, LandmarkPerId> & vins_landmark_db);
std::vector<camodocal::CameraPtr> cams;
};
Expand Down
4 changes: 3 additions & 1 deletion d2frontend/include/d2frontend/loop_detector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ namespace D2Common {
class VisualImageDesc;
struct LandmarkPerId;
struct VINSFrame;
using VINSFramePtr = std::shared_ptr<VINSFrame>;
}

namespace D2FrontEnd {
Expand All @@ -28,6 +29,7 @@ using D2Common::LandmarkPerId;
using D2Common::VisualImageDesc;
using D2Common::VisualImageDescArray;
using D2Common::VINSFrame;
using D2Common::VINSFramePtr;
using D2Common::Point2fVector;

class LoopCam;
Expand Down Expand Up @@ -116,7 +118,7 @@ class LoopDetector {
LoopCam * loop_cam = nullptr;
cv::Mat decode_image(const VisualImageDesc & _img_desc);
void updatebyLandmarkDB(const std::map<LandmarkIdType, LandmarkPerId> & vins_landmark_db);
void updatebySldWin(const std::vector<VINSFrame*> sld_win);
void updatebySldWin(const std::vector<VINSFramePtr> sld_win);
bool hasFrame(FrameIdType frame_id);

int databaseSize() const;
Expand Down
2 changes: 1 addition & 1 deletion d2frontend/src/d2featuretracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ D2FeatureTracker::D2FeatureTracker(D2FTConfig config) : _config(config) {
_config.sp_track_use_lk);
}

void D2FeatureTracker::updatebySldWin(const std::vector<VINSFrame *> sld_win) {
void D2FeatureTracker::updatebySldWin(const std::vector<VINSFramePtr> sld_win) {
// update by sliding window
const Guard lock(keyframe_lock);
const Guard guard2(lmanager_lock);
Expand Down
2 changes: 1 addition & 1 deletion d2frontend/src/loop_detector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -839,7 +839,7 @@ void LoopDetector::updatebyLandmarkDB(
}
}

void LoopDetector::updatebySldWin(const std::vector<VINSFrame *> sld_win) {
void LoopDetector::updatebySldWin(const std::vector<VINSFramePtr> sld_win) {
const std::lock_guard<std::mutex> lock(keyframe_database_mutex);
for (auto frame : sld_win) {
auto frame_id = frame->frame_id;
Expand Down
2 changes: 1 addition & 1 deletion d2pgo/src/d2pgo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -362,7 +362,7 @@ void D2PGO::rotInitial(const std::vector<Swarm::LoopEdge>& good_loops) {
}

void D2PGO::saveG2O(bool only_self) {
std::vector<D2BaseFrame*> frames;
std::vector<std::shared_ptr<D2BaseFrame>> frames;
for (auto frame_id : used_frames) {
if (only_self) {
if (state.getFramebyId(frame_id)->drone_id == self_id) {
Expand Down
4 changes: 2 additions & 2 deletions d2vins/src/d2vins_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,8 +102,8 @@ class D2VINSNode : public D2FrontEnd::D2Frontend {
estimator->updateSldwin(frame_desc.drone_id, frame_desc.sld_win_status);
}
if (frame_desc.matched_frame < 0) {
VINSFrame frame(frame_desc);
estimator->getVisualizer().pubFrame(&frame);
auto frame = std::make_shared<VINSFrame>(frame_desc);
estimator->getVisualizer().pubFrame(frame);
}
}
D2Frontend::processRemoteImage(frame_desc, succ_track);
Expand Down
86 changes: 42 additions & 44 deletions d2vins/src/estimator/d2estimator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -156,45 +156,45 @@ std::pair<bool, Swarm::Pose> D2Estimator::initialFramePnP(
return std::make_pair(success, pose_imu);
}

VINSFrame *D2Estimator::addFrame(VisualImageDescArray &_frame) {
VINSFramePtr D2Estimator::addFrame(VisualImageDescArray& vframe) {
// First we init corresponding pose for with IMU
auto &last_frame = state.lastFrame();
auto &lastvframe = state.lastFrame();
auto motion_predict = getMotionPredict(
_frame.stamp); // Redo motion predict for get latest initial pose
VINSFrame frame(_frame, motion_predict.second, last_frame);
vframe.stamp); // Redo motion predict for get latest initial pose
auto frame = std::make_shared<VINSFrame>(vframe, motion_predict.second, lastvframe);
if (params->init_method == D2VINSConfig::INIT_POSE_IMU) {
frame.odom = motion_predict.first;
frame->odom = motion_predict.first;
} else {
auto odom_imu = motion_predict.first;
auto pnp_init = initialFramePnP(_frame, last_frame.odom.pose());
auto pnp_init = initialFramePnP(vframe, lastvframe.odom.pose());
if (!pnp_init.first) {
// Use IMU
SPDLOG_WARN("Initialization failed, use IMU instead.");
} else {
odom_imu.pose() = pnp_init.second;
}
frame.odom = odom_imu;
frame->odom = odom_imu;
}
frame.odom.stamp = _frame.stamp;
frame.reference_frame_id = state.getReferenceFrameId();
frame->odom.stamp = vframe.stamp;
frame->referencevframe_id = state.getReferenceFrameId();

auto frame_ret = state.addFrame(_frame, frame);
state.addFrame(vframe, frame);
// Clear old frames after add
if (params->estimation_mode != D2Common::DISTRIBUTED_CAMERA_CONSENUS) {
margined_landmarks = state.clearUselessFrames(
isInitialized()); // Only marginlization when solved
}
_frame.setTd(state.getTd(_frame.drone_id));
vframe.setTd(state.getTd(vframe.drone_id));
// Assign IMU and initialization to VisualImageDescArray for broadcasting.
_frame.imu_buf = motion_predict.second.first;
_frame.pose_drone = frame.odom.pose();
_frame.Ba = frame.Ba;
_frame.Bg = frame.Bg;
_frame.reference_frame_id = frame.reference_frame_id;
vframe.imu_buf = motion_predict.second.first;
vframe.pose_drone = frame->odom.pose();
vframe.Ba = frame->Ba;
vframe.Bg = frame->Bg;
vframe.referencevframe_id = frame->referencevframe_id;

spdlog::debug("Initialize VINSFrame with {}: {}", params->init_method,
frame.toStr().c_str());
return frame_ret;
return frame;
}

void D2Estimator::addRemoteImuBuf(int drone_id, const IMUBuffer &imu_) {
Expand All @@ -221,7 +221,7 @@ void D2Estimator::addRemoteImuBuf(int drone_id, const IMUBuffer &imu_) {
}
}

VINSFrame *D2Estimator::addFrameRemote(const VisualImageDescArray &_frame) {
VINSFramePtr D2Estimator::addFrameRemote(const VisualImageDescArray &_frame) {
if (params->estimation_mode == D2Common::SOLVE_ALL_MODE ||
params->estimation_mode == D2Common::SERVER_MODE) {
addRemoteImuBuf(_frame.drone_id, _frame.imu_buf);
Expand All @@ -235,26 +235,26 @@ VINSFrame *D2Estimator::addFrameRemote(const VisualImageDescArray &_frame) {
params->estimation_mode == D2Common::SERVER_MODE) {
auto &imu_buf = imu_bufs.at(_frame.drone_id);
auto ret =
imu_buf.periodIMU(last_frame.imu_buf_index, _frame.stamp + state.getTd());
imu_buf.periodIMU(frame->imu_buf_index, _frame.stamp + state.getTd());
auto _imu = ret.first;
if (fabs(_imu.size() / (_frame.stamp - last_frame.stamp) -
if (fabs(_imu.size() / (_frame.stamp - frame->stamp) -
params->IMU_FREQ) > 15) {
SPDLOG_WARN(
"Remote IMU error freq: {:.3f} start_t "
"{:.3f}/{:.3f} end_t {:.3f}/{:.3f}",
_imu.size() / (_frame.stamp - last_frame.stamp),
last_frame.stamp + state.getTd(), _imu[0].t, _frame.stamp + state.getTd(),
_imu.size() / (_frame.stamp - frame->stamp),
frame->stamp + state.getTd(), _imu[0].t, _frame.stamp + state.getTd(),
_imu[_imu.size() - 1].t);
}
vinsframe = VINSFrame(_frame, ret, last_frame);
} else {
vinsframe = VINSFrame(_frame, _frame.Ba, _frame.Bg);
}
if (_frame.reference_frame_id != state.getReferenceFrameId()) {
auto ego_last = last_frame.initial_ego_pose;
auto ego_last = frame->initial_ego_pose;
auto pose_local_cur = _frame.pose_drone;
auto pred_cur_pose =
last_frame.odom.pose() * ego_last.inverse() * pose_local_cur;
frame->odom.pose() * ego_last.inverse() * pose_local_cur;
vinsframe.odom.pose() = pred_cur_pose;
spdlog::debug("Initial remoteframe {}@{} with ego-motion: {}",
_frame.frame_id, r_drone_id, pred_cur_pose.toStr());
Expand Down Expand Up @@ -294,7 +294,7 @@ VINSFrame *D2Estimator::addFrameRemote(const VisualImageDescArray &_frame) {

void D2Estimator::addSldWinToFrame(VisualImageDescArray &frame) {
for (unsigned int i = 0; i < state.size(); i++) {
frame.sld_win_status.push_back(state.getFrame(i).frame_id);
frame.sld_win_status.push_back(state.getFrame(i)->frame_id);
}
}

Expand Down Expand Up @@ -699,14 +699,12 @@ void D2Estimator::addIMUFactor(FrameIdType frame_ida, FrameIdType frame_idb,
void D2Estimator::setupImuFactors() {
if (state.size() > 1) {
for (size_t i = 0; i < state.size() - 1; i++) {
auto &frame_a = state.getFrame(i);
auto &frame_b = state.getFrame(i + 1);
auto pre_integrations = frame_b.pre_integrations; // Prev to current
// printf("IMU Factor %d<->%d prev %d\n", frame_a.frame_id,
// frame_b.frame_id, frame_b.prev_frame_id);
assert(frame_b.prev_frame_id == frame_a.frame_id &&
auto frame_a = state.getFrame(i);
auto frame_b = state.getFrame(i + 1);
auto pre_integrations = frame_b->pre_integrations; // Prev to current
assert(frame_b->prev_frame_id == frame_a->frame_id &&
"Wrong prev frame id");
addIMUFactor(frame_a.frame_id, frame_b.frame_id, pre_integrations);
addIMUFactor(frame_a->frame_id, frame_b->frame_id, pre_integrations);
}
}

Expand All @@ -719,17 +717,17 @@ void D2Estimator::setupImuFactors() {
}
if (state.size(drone_id) > 1) {
for (size_t i = 0; i < state.size(drone_id) - 1; i++) {
auto &frame_a = state.getFrame(drone_id, i);
auto &frame_b = state.getFrame(drone_id, i + 1);
auto frame_a = state.getFrame(drone_id, i);
auto frame_b = state.getFrame(drone_id, i + 1);
auto pre_integrations = frame_b.pre_integrations; // Prev to current
if (pre_integrations == nullptr) {
SPDLOG_WARN("frame {}<->{}@{} pre_integrations is nullptr.",
frame_a.frame_id, frame_b.frame_id, drone_id);
frame_a->frame_id, frame_b->frame_id, drone_id);
continue;
}
assert(frame_b.prev_frame_id == frame_a.frame_id &&
assert(frame_b->prev_frame_id == frame_a->frame_id &&
"Wrong prev frame id on remote");
addIMUFactor(frame_a.frame_id, frame_b.frame_id, pre_integrations);
addIMUFactor(frame_a->frame_id, frame_b->frame_id, pre_integrations);
}
}
}
Expand Down Expand Up @@ -882,7 +880,7 @@ const std::map<LandmarkIdType, LandmarkPerId> &D2Estimator::getLandmarkDB()
return state.getLandmarkDB();
}

const std::vector<VINSFrame *> &D2Estimator::getSelfSldWin() const {
const std::vector<VINSFramePtr> &D2Estimator::getSelfSldWin() const {
return state.getSldWin(self_id);
}

Expand Down Expand Up @@ -912,7 +910,7 @@ Swarm::Odometry D2Estimator::getOdometry() const {

Swarm::Odometry D2Estimator::getOdometry(int drone_id) const {
// Attention! We output IMU stamp!
auto odom = state.lastFrame(drone_id).odom;
auto odom = state.lastFrame(drone_id)->odom;
odom.stamp = odom.stamp + state.getTd();
return odom;
}
Expand Down Expand Up @@ -950,7 +948,7 @@ std::set<int> D2Estimator::getNearbyDronesbyPGOData(
// Check using D2VINS pose
state.lock_state();
if (state.size(p.first) > 0) {
auto d2vins_pose = state.lastFrame(p.first).odom.pose();
auto d2vins_pose = state.lastFrame(p.first)->odom.pose();
dist = (d2vins_pose.pos() - self_pose.pos()).norm();
dist_yaw = std::abs(d2vins_pose.yaw() - self_pose.yaw());
}
Expand Down Expand Up @@ -981,16 +979,16 @@ D2Estimator::getMotionPredict(double stamp) const {
if (!initFirstPoseFlag) {
return std::make_pair(Swarm::Odometry(), std::make_pair(IMUBuffer(), -1));
}
const auto &last_frame = state.lastFrame();
auto ret = imu_bufs.at(self_id).periodIMU(last_frame.imu_buf_index,
auto last_frame = state.lastFrame();
auto ret = imu_bufs.at(self_id).periodIMU(last_frame->imu_buf_index,
stamp + state.getTd());
auto _imu = ret.first;
auto index = ret.second;
if (fabs(_imu.size() / (stamp - last_frame.stamp) - params->IMU_FREQ) > 15) {
if (fabs(_imu.size() / (stamp - last_frame->stamp) - params->IMU_FREQ) > 15) {
SPDLOG_WARN(
"Local IMU error freq: {:.3f} start_t {:.3f}/{:.3f} end_t "
"{:.3f}/{:.3f}",
_imu.size() / (stamp - last_frame.stamp), last_frame.stamp + state.getTd(),
_imu.size() / (stamp - last_frame->stamp), last_frame->stamp + state.getTd(),
_imu[0].t, stamp + state.getTd(), _imu[_imu.size() - 1].t);
}
return std::make_pair(_imu.propagation(last_frame), ret);
Expand Down
6 changes: 3 additions & 3 deletions d2vins/src/estimator/d2estimator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,8 @@ class D2Estimator {

//Internal functions
bool tryinitFirstPose(VisualImageDescArray & frame);
VINSFrame * addFrame(VisualImageDescArray & _frame);
VINSFrame * addFrameRemote(const VisualImageDescArray & _frame);
VINSFramePtr addFrame(VisualImageDescArray & _frame);
VINSFramePtr addFrameRemote(const VisualImageDescArray & _frame);
void solveNonDistrib();
void setupImuFactors();
void setupLandmarkFactors();
Expand Down Expand Up @@ -94,7 +94,7 @@ class D2Estimator {
void sendSyncSignal(SyncSignal data, int64_t token);
bool readyForStart();
const std::map<LandmarkIdType, LandmarkPerId> & getLandmarkDB() const;
const std::vector<VINSFrame*> & getSelfSldWin() const;
const std::vector<VINSFramePtr> & getSelfSldWin() const;
D2Visualization & getVisualizer();
void setPGOPoses(const std::map<int, Swarm::Pose> & poses);
std::set<int> getNearbyDronesbyPGOData(const std::map<int, std::pair<int, Swarm::Pose>> & vins_poses);
Expand Down
Loading

0 comments on commit 4c0e48f

Please sign in to comment.