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 }
00106
00107 #endif // RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_ISAM_H