/** \brief Constructor
*/
FeatureTrackerNode(ros::NodeHandle& nh): nh_(nh), fsm_(&multiCamera_){
static_assert(l2>=l1, "l2 must be larger than l1");
subImu_ = nh_.subscribe("imuMeas", 1000, &FeatureTrackerNode::imuCallback,this);
subImg_ = nh_.subscribe("/cam0/image_raw", 1000, &FeatureTrackerNode::imgCallback,this);
min_feature_count_ = 50;
max_feature_count_ = 20; // Maximal number of feature which is added at a time (not total)
cv::namedWindow("Tracker");
multiCamera_.cameras_[0].load("/home/michael/calibrations/p22035_equidist.yaml");
fsm_.allocateMissing();
};
/** \brief Constructor
*/
FilterState():fsm_(nullptr), transformFeatureOutputCT_(nullptr), featureOutputCov_((int)(FeatureOutput::D_),(int)(FeatureOutput::D_)){
usePredictionMerge_ = true;
imgTime_ = 0.0;
imageCounter_ = 0;
groundtruth_qCJ_.setIdentity();
groundtruth_JrJC_.setZero();
groundtruth_qJI_.setIdentity();
groundtruth_IrIJ_.setZero();
groundtruth_qCB_.setIdentity();
groundtruth_BrBC_.setZero();
plotGroundtruth_ = true;
state_.initFeatureManagers(fsm_);
fsm_.allocateMissing();
drawPB_ = 1;
drawPS_ = mtState::patchSize_*pow(2,mtState::nLevels_-1)+2*drawPB_;
}
