diff --git a/d2common/include/d2common/d2baseframe.h b/d2common/include/d2common/d2baseframe.h index 0aed447..f911d6e 100644 --- a/d2common/include/d2common/d2baseframe.h +++ b/d2common/include/d2common/d2baseframe.h @@ -58,4 +58,7 @@ struct D2BaseFrame { return odom.pos(); } }; -} \ No newline at end of file + +using D2BaseFramePtr = std::shared_ptr; + +} // namespace D2Common diff --git a/d2common/include/d2common/solver/ARock.hpp b/d2common/include/d2common/solver/ARock.hpp index da24787..76953a0 100644 --- a/d2common/include/d2common/solver/ARock.hpp +++ b/d2common/include/d2common/solver/ARock.hpp @@ -70,6 +70,7 @@ class ARockSolver : public SolverWrapper, public ARockBase { void reset() override; void scanAndCreateDualStates() override; virtual void addResidual(const std::shared_ptr& residual_info) override; + virtual SolverReport solve(std::function func_set_properties) { return SolverReport();}; // TODO: rewrite use it SolverReport solve() override; void resetResiduals(); }; diff --git a/d2common/src/solver/ARock.cpp b/d2common/src/solver/ARock.cpp index 552b0e3..14441f0 100644 --- a/d2common/src/solver/ARock.cpp +++ b/d2common/src/solver/ARock.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include diff --git a/d2pgo/src/d2pgo.cpp b/d2pgo/src/d2pgo.cpp index b9887db..d47b558 100644 --- a/d2pgo/src/d2pgo.cpp +++ b/d2pgo/src/d2pgo.cpp @@ -12,33 +12,33 @@ namespace D2PGO { -void D2PGO::addFrame(D2BaseFrame frame) { +void D2PGO::addFrame(const D2BaseFramePtr& frame) { const Guard lock(state_lock); - if (state.hasFrame(frame.frame_id)) { + if (state.hasFrame(frame->frame_id)) { return; } - if (config.is_realtime && state.hasDrone(frame.drone_id)) { + if (config.is_realtime && state.hasDrone(frame->drone_id)) { // Use ego motion and current estimation to predict the frame pose - auto ego_motion = frame.initial_ego_pose; + auto ego_motion = frame->initial_ego_pose; auto cur_est_last_frame = - state.getFrames(frame.drone_id).back()->odom.pose(); + state.getFrames(frame->drone_id).back()->odom.pose(); auto ego_motion_last_frame = - state.getFrames(frame.drone_id).back()->initial_ego_pose; + state.getFrames(frame->drone_id).back()->initial_ego_pose; auto pose = cur_est_last_frame * Swarm::Pose::DeltaPose(ego_motion_last_frame, ego_motion); - frame.odom.pose() = pose; + frame->odom.pose() = pose; } state.addFrame(frame); // printf("[D2PGO@%d]add frame %ld ref %d ego_pose %s pose %s from drone - // %d\n", self_id, frame.frame_id, frame.reference_frame_id, - // frame.initial_ego_pose.toStr().c_str(), - // frame.odom.pose().toStr().c_str(), frame.drone_id); - if (ego_motion_trajs.find(frame.drone_id) == ego_motion_trajs.end()) { - Swarm::DroneTrajectory traj(frame.drone_id, true); - ego_motion_trajs[frame.drone_id] = traj; - } - ego_motion_trajs[frame.drone_id].push(frame.stamp, frame.initial_ego_pose, - frame.frame_id); + // %d\n", self_id, frame->frame_id, frame->reference_frame_id, + // frame->initial_ego_pose.toStr().c_str(), + // frame->odom.pose().toStr().c_str(), frame->drone_id); + if (ego_motion_trajs.find(frame->drone_id) == ego_motion_trajs.end()) { + Swarm::DroneTrajectory traj(frame->drone_id, true); + ego_motion_trajs[frame->drone_id] = traj; + } + ego_motion_trajs[frame->drone_id].push(frame->stamp, frame->initial_ego_pose, + frame->frame_id); updated = true; is_rot_init_convergence = false; } @@ -62,35 +62,35 @@ void D2PGO::addLoop(const Swarm::LoopEdge& loop_info, bool add_state_by_loop) { if (state.hasFrame(loop_info.keyframe_id_a) && !state.hasFrame(loop_info.keyframe_id_b)) { // Add frame idb to state - D2BaseFrame frame_desc; - frame_desc.drone_id = loop_info.id_b; - frame_desc.frame_id = loop_info.keyframe_id_b; - frame_desc.reference_frame_id = + auto frame_desc = std::make_shared(); + frame_desc->drone_id = loop_info.id_b; + frame_desc->frame_id = loop_info.keyframe_id_b; + frame_desc->reference_frame_id = state.getFramebyId(loop_info.keyframe_id_a)->reference_frame_id; // Initialize pose with known pose a and this loop edge - frame_desc.odom.pose() = + frame_desc->odom.pose() = state.getFramebyId(loop_info.keyframe_id_a)->odom.pose() * loop_info.relative_pose; addFrame(frame_desc); // printf("[D2PGO@%d]add frame %ld pose %s from drone %d\n", self_id, - // frame_desc.frame_id, - // frame_desc.odom.pose().toStr().c_str(), frame_desc.drone_id); + // frame_desc->frame_id, + // frame_desc->odom.pose().toStr().c_str(), frame_desc->drone_id); } else if (!state.hasFrame(loop_info.keyframe_id_a) && state.hasFrame(loop_info.keyframe_id_b)) { // Add frame idb to state - D2BaseFrame frame_desc; - frame_desc.drone_id = loop_info.id_a; - frame_desc.frame_id = loop_info.keyframe_id_a; - frame_desc.reference_frame_id = + auto frame_desc = std::make_shared(); + frame_desc->drone_id = loop_info.id_a; + frame_desc->frame_id = loop_info.keyframe_id_a; + frame_desc->reference_frame_id = state.getFramebyId(loop_info.keyframe_id_b)->reference_frame_id; // Initialize pose with known pose a and this loop edge - frame_desc.odom.pose() = + frame_desc->odom.pose() = state.getFramebyId(loop_info.keyframe_id_b)->odom.pose() * loop_info.relative_pose.inverse(); addFrame(frame_desc); // printf("[D2PGO@%d]add frame %ld pose %s from drone %d\n", self_id, - // frame_desc.frame_id, - // frame_desc.odom.pose().toStr().c_str(), frame_desc.drone_id); + // frame_desc->frame_id, + // frame_desc->odom.pose().toStr().c_str(), frame_desc->drone_id); } } updated = true; @@ -692,7 +692,7 @@ void D2PGO::sendSignal(const std::string& signal) { bd_signal_callback(signal); } -std::vector D2PGO::getAllLocalFrames() { +std::vector D2PGO::getAllLocalFrames() { const Guard lock(state_lock); return state.getFrames(self_id); } diff --git a/d2pgo/src/d2pgo.h b/d2pgo/src/d2pgo.h index f8a1431..d4077c2 100644 --- a/d2pgo/src/d2pgo.h +++ b/d2pgo/src/d2pgo.h @@ -58,7 +58,7 @@ class D2PGO { available_robots{_config.self_id} { } void evalLoop(const Swarm::LoopEdge & loop); - void addFrame(D2BaseFrame frame_desc); + void addFrame(const D2BaseFramePtr& frame_desc); void addLoop(const Swarm::LoopEdge & loop_info, bool add_state_by_loop=false); void setStateProperties(ceres::Problem & problem); bool solve_multi(bool force_solve=false); @@ -69,7 +69,7 @@ class D2PGO { void sendSignal(const std::string & signal); void rotInitial(const std::vector & good_loops); std::map getOptimizedTrajs(); - std::vector getAllLocalFrames(); + std::vector getAllLocalFrames(); void setAvailableRobots(const std::set & _available_robots) { available_robots = _available_robots; } diff --git a/d2pgo/src/d2pgo_node.cpp b/d2pgo/src/d2pgo_node.cpp index b5b144b..1ee6b62 100644 --- a/d2pgo/src/d2pgo_node.cpp +++ b/d2pgo/src/d2pgo_node.cpp @@ -33,7 +33,7 @@ class D2PGONode { protected: void processImageArray(const swarm_msgs::VIOFrame &vioframe) { if (vioframe.is_keyframe) { - D2BaseFrame frame(vioframe); + auto frame = std::make_shared(vioframe); pgo->addFrame(frame); } } diff --git a/d2pgo/src/pgostate.hpp b/d2pgo/src/pgostate.hpp index d7b2aee..493b4e1 100644 --- a/d2pgo/src/pgostate.hpp +++ b/d2pgo/src/pgostate.hpp @@ -7,7 +7,7 @@ using namespace D2Common; namespace D2PGO { class PGOState : public D2State { protected: - std::map> drone_frames; + std::map> drone_frames; std::map ego_drone_trajs; std::map initial_attitude; @@ -19,40 +19,38 @@ class PGOState : public D2State { } else { printf("[D2PGO] PGOState: is 6dof\n"); } - drone_frames[self_id] = std::vector(); + drone_frames[self_id] = std::vector(); } - void addFrame(const D2BaseFrame & _frame) { + void addFrame(const D2BaseFramePtr & frame) { const Guard lock(state_lock); // printf("[D2PGO@%d] PGOState: add frame %ld for drone %d: %s\n", self_id, - // _frame.frame_id, _frame.drone_id, _frame.odom.pose().toStr().c_str()); - all_drones.insert(_frame.drone_id); - auto * frame = new D2BaseFrame; - *frame = _frame; + // frame->frame_id, frame->drone_id, frame->odom.pose().toStr().c_str()); + all_drones.insert(frame->drone_id); frame_db[frame->frame_id] = frame; if (is_4dof) { _frame_pose_state[frame->frame_id] = makeSharedStateArray(POSE4D_SIZE); - _frame.odom.pose().to_vector_xyzyaw(_frame_pose_state[frame->frame_id]); + frame->odom.pose().to_vector_xyzyaw(_frame_pose_state[frame->frame_id]); } else { _frame_pose_state[frame->frame_id] = makeSharedStateArray(POSE_SIZE); _frame_rot_state[frame->frame_id] = makeSharedStateArray(ROTMAT_SIZE); _frame_pose_pertub_state[frame->frame_id] = makeSharedStateArray(POSE_EFF_SIZE); - _frame.odom.pose().to_vector(_frame_pose_state[frame->frame_id]); + frame->odom.pose().to_vector(_frame_pose_state[frame->frame_id]); Map> rot(CheckGetPtr(_frame_rot_state[frame->frame_id])); - rot = _frame.odom.pose().R(); + rot = frame->odom.pose().R(); Map pose_pertub(CheckGetPtr(_frame_pose_pertub_state[frame->frame_id])); pose_pertub.setZero(); - pose_pertub.segment<3>(0) = _frame.T(); + pose_pertub.segment<3>(0) = frame->T(); - initial_attitude[frame->frame_id] = _frame.odom.att(); + initial_attitude[frame->frame_id] = frame->odom.att(); } - if (drone_frames.find(_frame.drone_id) == drone_frames.end()) { - drone_frames[_frame.drone_id] = std::vector(); - ego_drone_trajs[_frame.drone_id] = Swarm::DroneTrajectory(); + if (drone_frames.find(frame->drone_id) == drone_frames.end()) { + drone_frames[frame->drone_id] = std::vector(); + ego_drone_trajs[frame->drone_id] = Swarm::DroneTrajectory(); } - drone_frames.at(_frame.drone_id).push_back(frame); - ego_drone_trajs[_frame.drone_id].push(frame->stamp, frame->initial_ego_pose, frame->frame_id); + drone_frames.at(frame->drone_id).push_back(frame); + ego_drone_trajs[frame->drone_id].push(frame->stamp, frame->initial_ego_pose, frame->frame_id); } int size(int drone_id) { @@ -62,7 +60,7 @@ class PGOState : public D2State { return drone_frames.at(drone_id).size(); } - std::vector & getFrames(int drone_id) { + std::vector & getFrames(int drone_id) { return drone_frames.at(drone_id); } diff --git a/d2pgo/test/d2pgo_test.cpp b/d2pgo/test/d2pgo_test.cpp index 5380fd9..f68309b 100644 --- a/d2pgo/test/d2pgo_test.cpp +++ b/d2pgo/test/d2pgo_test.cpp @@ -40,7 +40,7 @@ class D2PGOTester { std::map path_pubs; std::vector edges; - std::map keyframeid_agent_pose; + std::map keyframeid_agent_pose; std::map buf_for_simulate_delay; public: diff --git a/d2pgo/test/posegraph_g2o.cpp b/d2pgo/test/posegraph_g2o.cpp index 6936935..60975e0 100644 --- a/d2pgo/test/posegraph_g2o.cpp +++ b/d2pgo/test/posegraph_g2o.cpp @@ -128,7 +128,7 @@ bool match_edge_se3(std::string line, int& agent_ida, FrameIdType& ida, } void read_g2o_agent(std::string path, - std::map& keyframeid_agent_pose, + std::map& keyframeid_agent_pose, std::vector& edges, bool is_4dof, int max_agent_id, int drone_id, bool ignore_infor) { std::ifstream infile(path); @@ -141,12 +141,12 @@ void read_g2o_agent(std::string path, auto success = match_vertex_se3(line, agent_id, id_a, pose, max_agent_id); if (success) { // Add new vertex here - D2BaseFrame frame; - frame.drone_id = agent_id; - frame.odom.pose() = pose; - frame.initial_ego_pose = pose; - frame.frame_id = id_a; - frame.reference_frame_id = 0; + D2BaseFramePtr frame = std::make_shared(); + frame->drone_id = agent_id; + frame->odom.pose() = pose; + frame->initial_ego_pose = pose; + frame->frame_id = id_a; + frame->reference_frame_id = 0; keyframeid_agent_pose[id_a] = frame; } else { Eigen::Matrix6d information; @@ -172,7 +172,7 @@ void read_g2o_agent(std::string path, void read_g2o_multi_agents( std::string path, - std::map>& keyframeid_agent_pose, + std::map>& keyframeid_agent_pose, std::map>& edges, G2oParseParam param) { auto files = get_all(path, ".g2o"); std::sort(files.begin(), files.end()); @@ -182,7 +182,7 @@ void read_g2o_multi_agents( break; } auto file = files[i].second; - keyframeid_agent_pose[i] = std::map(); + keyframeid_agent_pose[i] = std::map(); edges[i] = std::vector(); read_g2o_agent(file, keyframeid_agent_pose[i], edges[i], param.is_4dof, param.agents_num - 1); @@ -192,7 +192,7 @@ void read_g2o_multi_agents( } void write_result_to_g2o(const std::string& path, - const std::vector& frames, + const std::vector& frames, const std::vector& edges, bool write_ego_pose) { std::fstream file; diff --git a/d2pgo/test/posegraph_g2o.hpp b/d2pgo/test/posegraph_g2o.hpp index ccd66e9..78175fd 100644 --- a/d2pgo/test/posegraph_g2o.hpp +++ b/d2pgo/test/posegraph_g2o.hpp @@ -24,19 +24,19 @@ struct G2oParseParam { void read_g2o_agent( std::string path, - std::map & keyframeid_agent_pose, + std::map & keyframeid_agent_pose, std::vector & edges, bool is_4dof, int max_agent_id=100000, int drone_id=-1, bool ignore_infor=false); void read_g2o_multi_agents( std::string path, - std::map> & keyframeid_agent_pose, + std::map> & keyframeid_agent_pose, std::map> & edges, G2oParseParam param ); void write_result_to_g2o(const std::string & path, - const std::vector & frames, + const std::vector & frames, const std::vector & edges, bool write_ego_pose=false); } \ No newline at end of file diff --git a/d2pgo/test/test_pgo.cpp b/d2pgo/test/test_pgo.cpp index 4eee153..a9c3793 100644 --- a/d2pgo/test/test_pgo.cpp +++ b/d2pgo/test/test_pgo.cpp @@ -15,10 +15,10 @@ void testDummy() { config.main_id = 0; config.loop_distance_threshold = 10000; D2PGO::D2PGO* pgo = new D2PGO::D2PGO(config); - D2BaseFrame frame0(0.0, 0, 0, 0, true, Swarm::Pose::Identity()); - D2BaseFrame frame1(1.0, 1, 0, 0, true, + auto frame0 = std::make_shared(0.0, 0, 0, 0, true, Swarm::Pose::Identity()); + auto frame1 = std::make_shared(1.0, 1, 0, 0, true, Swarm::Pose(Vector3d(1, 0, 0), Quaterniond(1, 0, 0, 0))); - D2BaseFrame frame2(2.0, 2, 0, 0, true, + auto frame2 = std::make_shared(2.0, 2, 0, 0, true, Swarm::Pose(Vector3d(2, 0, 0), Quaterniond(1, 0, 0, 0))); Swarm::LoopEdge edge(0, 1, Swarm::Pose(Vector3d(1, 0, 0), Quaterniond(1, 0, 0, 0)),