monoslam.hpp

Go to the documentation of this file.

#ifndef _THERMALVIS_MONOSLAM_H_
#define _THERMALVIS_MONOSLAM_H_
        
#include "general_resources.hpp"
#include "ros_resources.hpp"
#include "opencv_resources.hpp"
#include "pcl_resources.hpp"

#include "improc.hpp"
#include "video.hpp"
#include "features.hpp"
#include "reconstruction.hpp"
#include "sba.hpp"
#include "keyframes.hpp"
#include "geometry.hpp"

#include "feature_tracks.h"

#include "monoslamConfig.h"

typedef dynamic_reconfigure::Server < thermalvis::monoslamConfig > Server;

// Drawing
#define DEFAULT_DRAWGRAPH_DECIMATION                    1
#define DEFAULT_DRAWGRAPH_BICOLOR                               0

// Hard Limits
#define MAX_FRAMES                                                              1000
#define MAX_TRACKS                                                              10000
#define SBA_MEMORY                                                              134217728
#define MAX_SEGMENTS                                                    16
#define INITIALIZATION_SCORING_PARAMETERS               5

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

const char __PROGRAM__[] = "THERMALVIS_MONOSLAM";

bool wantsToShutdown = false;
void mySigintHandler(int sig);

struct slamData {
        
        string stream;
        
        double motionThreshold;
        int adjustmentFrames;
        int flowback;
        double minStartupScore;
        cameraParameters cameraData;
        bool verboseMode;
        bool obtainStartingData(ros::NodeHandle& nh);
        
        double dataTimeout;
        
        int keyframeSpacing;
        
        int maxGuides;
        int maxKeyframeSeparation;
        
        int framesForTriangulation;
        
        int timeSpacing;
        
        int minTrackedFeatures;
        
        int min3dPtsForPnP;
        
        int maxInitializationFrames;

        
        double minGeometryScore;
        double minKeyframeScore;
        
        double requiredTrackFrac;
        
        int camerasPerSys;
        
        bool timeDebug;
        
        bool logErrors;
        
        int maxTestsPerFrame;
        
        int initialStructureIterations, poseEstimateIterations, fullSystemIterations, subsequenceIterations, keyframeIterations;
        
        bool keyframeEvaluationMode; 
        
        int minStartingSeparation, maxStartingSeparation;
        
        string read_addr, evaluationFile, errorFile, initializationScorecard;
        
};

struct timeAnalyzer {
        
        int cycles;
        double vals[1024];
        double average;
        double sigma;
        
        struct timeval cycle_timer;
        
        timeAnalyzer();
        void calcParameters();
        void startRecording();
        void stopRecording();
        
};

class slamNode {
private:

        int decimation;
        int bicolor;

        bool isTracking;
        
        bool f1;
        
        bool evaluationCompleted;

        unsigned int baseConnectionNum;
        
        double **scorecardParams;

        slamData configData;
        
        ofstream evaluationStream;
        
        unsigned int nextFrame;
        
        unsigned int putativelyEstimatedFrames;
        
        cv::Mat eye4;
        
        bool repetitionNoted;
        
        ofstream error_file;
        char error_filename[256];

        ros::Subscriber tracks_sub;
        ros::Subscriber info_sub;
        
        SysSBA sys, display_sys;
        ros::Publisher path_pub;
        ros::Publisher camera_pub;
        ros::Publisher points_pub;
        
        ros::Timer timer;
        
        bool firstIteration;
        
        cv::Mat blank;
        
        bool structureValid, structureFormed;
        
        int latestFrame;
        
        bool infoProcessed;
        
        struct timeval cycle_timer;
        double elapsedTime;
        
        double distanceConstraint;
        
        vector<featureTrack> featureTrackVector;
        
        cv::Point3d cloudCentroid[MAX_SEGMENTS], cloudStdDeviation[MAX_SEGMENTS];
        keyframeStore keyframe_store;
        cv::Mat F_arr[MAX_FRAMES], H_arr[MAX_FRAMES];
        
        cv::Mat ACM[MAX_FRAMES], ACM2[MAX_FRAMES];
        
        cv::Mat keyframeTestScores, keyframeTestFlags;
        
        char nodeName[256];
        
        ros::Publisher pose_pub;
        char pose_pub_name[256];
        
        geometry_msgs::PoseStamped currentPose;
        
        int currentPoseIndex;
        
        double minimumKeyframeScore;
        
        vector<unsigned int> framesReceived;
        
        unsigned int lastBasePose;
        
        unsigned int startingTracksCount;
        
        timeAnalyzer trackHandlingTime;
        timeAnalyzer triangulationTime;
        timeAnalyzer poseEstimationTime;
        timeAnalyzer bundleAdjustmentTime;

        // Thread-protection
        boost::mutex cam_mutex;
        boost::mutex tracks_mutex;
        boost::mutex keyframes_mutex;
        boost::mutex main_mutex;
        
        dynamic_reconfigure::Server<thermalvis::monoslamConfig> server;
        dynamic_reconfigure::Server<thermalvis::monoslamConfig>::CallbackType f;
        
public:

        slamNode(ros::NodeHandle& nh, slamData startupData);
        void assignPose(geometry_msgs::PoseStamped& pPose, cv::Mat& C);
        
        void estimatePose(vector<unsigned int>& basisNodes, unsigned int idx);
        
        void getBasisNodes(vector<unsigned int>& basisNodes, unsigned int idx);
        
        // Callbacks
        void handle_tracks(const thermalvis::feature_tracksConstPtr& msg);
        void handle_info(const sensor_msgs::CameraInfoConstPtr& info_msg);
        void main_loop(const ros::TimerEvent& event);
        
        // Routines
        bool formInitialStructure();
        bool findStartingFrames();
        void refreshPoses();
        bool checkForKeyframe();
        void processNextFrame();
        void update_display();
        void show_poses();
        void update_cameras_to_pnp();
        double assignStartingFrames(unsigned int best_iii, unsigned int best_jjj, double* keyframe_scores, cv::Mat& startingTrans);
        
        void getGuidingPose(cv::Mat *srcs, cv::Mat& dst, unsigned int idx);
        
        void clearSystem();
        
        void processScorecard();
        
        void serverCallback(thermalvis::monoslamConfig &config, uint32_t level);
        
        // Misc
        bool stillTracking() {
                return isTracking;
        }
        
        void prepareForTermination();
        
};

boost::shared_ptr < slamNode > *globalNodePtr;

#endif