Refactor to shared_ptr: part3.2 change VINSFrame* to shared_ptr: fini…

…sh D2PGO.

d2common/include/d2common/d2baseframe.h

@@ -58,4 +58,7 @@ struct D2BaseFrame {
         return odom.pos();
     }
 };
-}
+
+using D2BaseFramePtr = std::shared_ptr<D2Common::D2BaseFrame>;
+
+} // namespace D2Common

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<ResidualInfo>& residual_info) override;
+    virtual SolverReport solve(std::function<void()> func_set_properties) { return SolverReport();}; // TODO: rewrite use it
     SolverReport solve() override;
     void resetResiduals();
 };

d2common/src/solver/ARock.cpp

@@ -1,6 +1,7 @@
 #include <ceres/normal_prior.h>
 #include <d2common/solver/consenus_factor.h>
 #include <d2common/solver/consenus_factor_4d.h>
+#include <spdlog/spdlog.h>
 
 #include <d2common/solver/ARock.hpp>
 

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<D2BaseFrame>();
+      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<D2BaseFrame>();
+      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<D2BaseFrame*> D2PGO::getAllLocalFrames() {
+std::vector<D2BaseFramePtr> D2PGO::getAllLocalFrames() {
   const Guard lock(state_lock);
   return state.getFrames(self_id);
 }

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<Swarm::LoopEdge> & good_loops);
     std::map<int, Swarm::DroneTrajectory> getOptimizedTrajs();
-    std::vector<D2BaseFrame*> getAllLocalFrames();
+    std::vector<D2BaseFramePtr> getAllLocalFrames();
     void setAvailableRobots(const std::set<int> & _available_robots) {
         available_robots = _available_robots;
     }

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<D2BaseFrame>(vioframe);
       pgo->addFrame(frame);
     }
   }

d2pgo/src/pgostate.hpp

@@ -7,7 +7,7 @@ using namespace D2Common;
 namespace D2PGO {
 class PGOState : public D2State {
 protected:
-    std::map<int, std::vector<D2BaseFrame*>> drone_frames;
+    std::map<int, std::vector<D2BaseFramePtr>> drone_frames;
     std::map<int, Swarm::DroneTrajectory> ego_drone_trajs;
     std::map<int, Eigen::Quaterniond> initial_attitude;
 
@@ -19,40 +19,38 @@ class PGOState : public D2State {
         } else {
             printf("[D2PGO] PGOState: is 6dof\n");
         }
-        drone_frames[self_id] = std::vector<D2BaseFrame*>();
+        drone_frames[self_id] = std::vector<D2BaseFramePtr>();
     }
 
-    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<Matrix<state_type, 3, 3, RowMajor>> rot(CheckGetPtr(_frame_rot_state[frame->frame_id]));
-            rot = _frame.odom.pose().R();
+            rot = frame->odom.pose().R();
 
             Map<Eigen::Vector6d> 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<D2BaseFrame*>();
-            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<D2BaseFramePtr>();
+            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<D2BaseFrame*> & getFrames(int drone_id) {
+    std::vector<D2BaseFramePtr> & getFrames(int drone_id) {
         return drone_frames.at(drone_id);
     }
 

d2pgo/test/d2pgo_test.cpp

@@ -40,7 +40,7 @@ class D2PGOTester {
 
   std::map<int, ros::Publisher> path_pubs;
   std::vector<Swarm::LoopEdge> edges;
-  std::map<FrameIdType, D2BaseFrame> keyframeid_agent_pose;
+  std::map<FrameIdType, D2BaseFramePtr> keyframeid_agent_pose;
   std::map<double, D2Common::DPGOData> buf_for_simulate_delay;
 
  public:

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<FrameIdType, D2BaseFrame>& keyframeid_agent_pose,
+                    std::map<FrameIdType, D2BaseFramePtr>& keyframeid_agent_pose,
                     std::vector<Swarm::LoopEdge>& 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<D2BaseFrame>();
+      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<int, std::map<FrameIdType, D2BaseFrame>>& keyframeid_agent_pose,
+    std::map<int, std::map<FrameIdType, D2BaseFramePtr>>& keyframeid_agent_pose,
     std::map<int, std::vector<Swarm::LoopEdge>>& 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<FrameIdType, D2BaseFrame>();
+    keyframeid_agent_pose[i] = std::map<FrameIdType, D2BaseFramePtr>();
     edges[i] = std::vector<Swarm::LoopEdge>();
     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<D2BaseFrame*>& frames,
+                         const std::vector<D2BaseFramePtr>& frames,
                          const std::vector<Swarm::LoopEdge>& edges,
                          bool write_ego_pose) {
   std::fstream file;

d2pgo/test/posegraph_g2o.hpp

@@ -24,19 +24,19 @@ struct G2oParseParam {
 
 void read_g2o_agent(
     std::string path,
-    std::map<FrameIdType, D2BaseFrame> & keyframeid_agent_pose,
+    std::map<FrameIdType, D2BaseFramePtr> & keyframeid_agent_pose,
     std::vector<Swarm::LoopEdge> & 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<int, std::map<FrameIdType, D2BaseFrame>> & keyframeid_agent_pose,
+    std::map<int, std::map<FrameIdType, D2BaseFramePtr>> & keyframeid_agent_pose,
     std::map<int, std::vector<Swarm::LoopEdge>> & edges,
     G2oParseParam param
 );
 
 void write_result_to_g2o(const std::string & path, 
-    const std::vector<D2BaseFrame*> & frames, 
+    const std::vector<D2BaseFramePtr> & frames, 
     const std::vector<Swarm::LoopEdge> & edges,
     bool write_ego_pose=false);
 }

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<D2BaseFrame>(0.0, 0, 0, 0, true, Swarm::Pose::Identity());
+  auto frame1 = std::make_shared<D2BaseFrame>(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<D2BaseFrame>(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)),