lib_rgbdtools: keyframe_graph_solver

Go to the documentation of this file.
00001 
00024 #ifndef RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_ISAM_H
00025 #define RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_ISAM_H
00026 
00027 #include "rgbdtools/rgbd_keyframe.h"
00028 #include "rgbdtools/graph/keyframe_association.h"
00029 #include "rgbdtools/graph/keyframe_graph_solver.h"
00030 #include "isam/isam.h"
00031 #include "aruco/aruco.h"
00032 
00033 namespace rgbdtools {
00034 
00035 
00036 class KeyframeGraphSolverISAM: public KeyframeGraphSolver
00037 {
00038 
00039   public:
00040 
00043     KeyframeGraphSolverISAM();
00044 
00047     ~KeyframeGraphSolverISAM();
00048 
00049      void solve(
00050       KeyframeVector& keyframes,
00051       const KeyframeAssociationVector& associations);
00052      void solve_Iterative(KeyframeVector& keyframes);
00053      void solve_rgb_Iterative(KeyframeVector& keyframes);
00054 
00055      void solve(
00056       KeyframeVector& keyframes,
00057       const KeyframeAssociationVector& associations,
00058       AffineTransformVector& path);
00059      void save_graph(std::string filepath);
00060      void giveMarkers(std::vector<aruco::Marker> markers);
00061      void save_landmarks(std::string filepath);
00062 
00063      void parse (std::string fname,rgbdtools::KeyframeVector keyframes);
00064      void parse_line(char* str);
00065      void load_landmarks(std::string filepath);
00066      u_int getNOriginalPoses(){ return n_original_poses;}
00067 
00068      typedef  std::pair<int,int> Observation;
00069 
00070      u_int n_original_poses;
00071      std::map<Observation,isam::Point3d_Node*> landmarks;
00072      std::vector<isam::Point3d_Node*> landmarks_explicit;
00073      std::vector<isam::Point3d_Node*> marker_poses;
00074      std::vector<int> marker_kf;
00075      std::vector<isam::Pose3d_Point3d_Factor*> marker_observations;
00076      std::vector<aruco::Marker> markers_;
00077      std::vector<isam::Pose3d_Node*> poses;
00078      std::vector<bool> valid_poses;
00079      std::vector<bool> valid_markers;
00080      std::vector<isam::Pose3d_Pose3d_Factor*> vodom;
00081      std::vector<isam::Factor*> priors;
00082      std::vector<isam::Pose3d_Point3d_Factor*> landmark_observations;
00083      isam::Slam slam ;
00084      void optimizeGraph();     
00085      void getOptimizedPoses(AffineTransformVector& poses_);
00086      void add_vo(isam::Pose3d_Node *from, isam::Pose3d_Node *to, AffineTransform pose, isam::Noise noise); 
00087      void add_prior(isam::Pose3d_Node* pose_node, double x, double y, double z, double roll, double pitch, double yaw,isam::Noise noise);
00088      void add_pose(Pose pose,isam::Noise noise);
00089      void add_pose(double x, double y, double z, double roll, double pitch, double yaw,isam::Noise noise);
00090      void add_edge(isam::Pose3d_Node* from, isam::Pose3d_Node* to,double x, double y, double z, double roll, double pitch, double yaw, isam::Noise noise);
00091      void add_edge(isam::Pose3d_Node* from, isam::Pose3d_Node* to, const AffineTransform& pose, isam::Noise noise);
00092      void add_edges(u_int kf_idx,KeyframeVector& keyframes,
00093                                              const KeyframeAssociationVector& associations);
00094      void add_landmark(KeyframeVector& keyframes,KeyframeAssociation association, int from_idx,int to_idx,int j, isam::Noise noise3);
00095      void add_landmark_from_file(int from_idx,int to_idx,double x, double y, double z,isam::Noise noise3);
00096      void add_aruco(aruco::Marker marker, isam::Pose3d_Node* from, isam::Noise noise3);
00097      void getArucoPos(std::vector<std::vector<double> >* positions,std::vector<int>* marker_kfs,KeyframeVector& keyframes);
00098 
00099 private:
00100      void parse_factors();
00101      void parse_nodes();
00102 
00103 };
00104 
00105 } // namespace rgbdtools
00106 
00107 #endif // RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_ISAM_H