diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp index 25d7fd4067c2401b46db2c247961b54a8213f71f..d50eaf29481885942bfaa1812d90e3fbc73d6aa3 100644 --- a/hello_wolf/processor_range_bearing.cpp +++ b/hello_wolf/processor_range_bearing.cpp @@ -47,7 +47,8 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) // 2. cast incoming capture to the range-and-bearing type, add it to the keyframe CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture); - kf->addCapture(capture_rb); + // kf->addCapture(capture_rb); + capture_rb->link(kf); // 3. explore all observations in the capture for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++) @@ -70,9 +71,10 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) { // new landmark: // - create landmark - lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing)); + // lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing)); + lmk = std::static_pointer_cast<LandmarkPoint2D>(LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing))); WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose()); - getProblem()->getMap()->addLandmark(lmk); + // getProblem()->getMap()->addLandmark(lmk); // - add to known landmarks known_lmks.emplace(id, lmk); } @@ -81,17 +83,23 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) Vector2s rb(range,bearing); auto ftr = std::make_shared<FeatureRangeBearing>(rb, getSensor()->getNoiseCov()); - capture_rb->addFeature(ftr); + // capture_rb->addFeature(ftr); + FeatureBase::emplace<FeatureRangeBearing>(capture_rb, rb, getSensor()->getNoiseCov()); // 6. create factor auto prc = shared_from_this(); - auto ctr = std::make_shared<FactorRangeBearing>(capture_rb, - lmk, - prc, - false, - FAC_ACTIVE); - ftr->addFactor(ctr); - lmk->addConstrainedBy(ctr); + // auto ctr = std::make_shared<FactorRangeBearing>(capture_rb, + // lmk, + // prc, + // false, + // FAC_ACTIVE); + auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb, + lmk, + prc, + false, + FAC_ACTIVE); + // ftr->addFactor(ctr); + // lmk->addConstrainedBy(ctr); } } diff --git a/include/base/node_base.h b/include/base/node_base.h index d1eafaf887043876c52bbd20d11ff8fb206d050b..5782d9fc7092593ac3f6b13821adcb89f119aed6 100644 --- a/include/base/node_base.h +++ b/include/base/node_base.h @@ -83,8 +83,6 @@ class NodeBase void setName(const std::string& _name); ProblemPtr getProblem() const; virtual void setProblem(ProblemPtr _prob_ptr); - // template<typename classReturn, typename classType, typename... T> - // static std::shared_ptr<classReturn> emplace(T&&... all); }; } // namespace wolf diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h index da1393232145b65cd46a2c23c27f964049ca04c6..d18c9591e5abd9932a7781144f172b9a72826487 100644 --- a/include/base/processor/processor_odom_2D.h +++ b/include/base/processor/processor_odom_2D.h @@ -80,19 +80,8 @@ class ProcessorOdom2D : public ProcessorMotion // Factory method public: static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr); - void link(SensorBasePtr); - template<typename classType, typename... T> - static std::shared_ptr<ProcessorOdom2D> emplace(SensorBasePtr _sen_ptr, T&&... all); }; -template<typename classType, typename... T> -std::shared_ptr<ProcessorOdom2D> ProcessorOdom2D::emplace(SensorBasePtr _sen_ptr, T&&... all) -{ - ProcessorOdom2DPtr prc = std::make_shared<classType>(std::forward<T>(all)...); - prc->link(_sen_ptr); - return prc; -} - inline Eigen::VectorXs ProcessorOdom2D::deltaZero() const { return Eigen::VectorXs::Zero(delta_size_); diff --git a/src/capture/capture_GPS_fix.cpp b/src/capture/capture_GPS_fix.cpp index ccf9b8631bd0759448ec6c20da7b00b9f534087d..e24a43d5a51459aa2583ebca9f9e794ec08f9360 100644 --- a/src/capture/capture_GPS_fix.cpp +++ b/src/capture/capture_GPS_fix.cpp @@ -25,10 +25,11 @@ CaptureGPSFix::~CaptureGPSFix() void CaptureGPSFix::process() { // EXTRACT AND ADD FEATURES - addFeature(std::make_shared<FeatureGPSFix>(data_,data_covariance_)); + // addFeature(std::make_shared<FeatureGPSFix>(data_,data_covariance_)); // ADD CONSTRAINT - getFeatureList().front()->addFactor(std::make_shared <FactorGPS2D>(getFeatureList().front())); + // getFeatureList().front()->addFactor(std::make_shared <FactorGPS2D>(getFeatureList().front())); + FactorBase::emplace<FactorGPS2D>(getFeatureList().front(), getFeatureList().front()); } } //namespace wolf diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index 3d8a9fed3b5dd3570df735d7136902453390e33a..71b536f9c1780ef2a94a3f46898aadb152d79ccb 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -99,8 +99,6 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr) { //std::cout << "Adding feature" << std::endl; feature_list_.push_back(_ft_ptr); - _ft_ptr->setCapturePtr(shared_from_this()); - _ft_ptr->setProblem(getProblem()); return _ft_ptr; } diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp index 4f5cf88a4670649f8e61df9423353851797d424b..fc45eea814bc120ab3d7ec44867fcda407c3d4e5 100644 --- a/src/capture/capture_pose.cpp +++ b/src/capture/capture_pose.cpp @@ -18,14 +18,15 @@ CapturePose::~CapturePose() void CapturePose::emplaceFeatureAndFactor() { // Emplace feature - FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_); - addFeature(feature_pose); + // FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_); + // addFeature(feature_pose); + auto feature_pose = FeatureBase::emplace<FeaturePose>(shared_from_this(), data_, data_covariance_); // Emplace factor if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 ) - feature_pose->addFactor(std::make_shared<FactorPose2D>(feature_pose)); + FactorBase::emplace<FactorPose2D>(feature_pose, feature_pose); else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 ) - feature_pose->addFactor(std::make_shared<FactorPose3D>(feature_pose)); + FactorBase::emplace<FactorPose3D>(feature_pose, feature_pose); else throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D."); } diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp index 81900c7ef6077fe947f09c9f59a42d094c268a40..be1a73a7de7e5efe3be0c88677bae1f79455dcbd 100644 --- a/src/examples/test_processor_tracker_landmark.cpp +++ b/src/examples/test_processor_tracker_landmark.cpp @@ -62,18 +62,21 @@ int main() // Wolf problem ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D"); - SensorBasePtr sensor_ptr_ = std::make_shared< SensorBase>("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), - std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), - std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); - + // SensorBasePtr sensor_ptr_ = std::make_shared< SensorBase>("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), + // std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), + // std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); + auto sensor_ptr_ = SensorBase::emplace<SensorBase>(wolf_problem_ptr_->getHardware(), "ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), + std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), + std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); ProcessorParamsTrackerLandmarkPtr params_trk = std::make_shared<ProcessorParamsTrackerLandmark>(); params_trk->max_new_features = 5; params_trk->min_features_for_keyframe = 7; params_trk->time_tolerance = 0.25; - std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(params_trk); - - wolf_problem_ptr_->addSensor(sensor_ptr_); - sensor_ptr_->addProcessor(processor_ptr_); + // std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(params_trk); + std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = + std::static_pointer_cast<ProcessorTrackerLandmarkDummy>(ProcessorBase::emplace<ProcessorTrackerLandmarkDummy>(sensor_ptr_, params_trk)); + // wolf_problem_ptr_->addSensor(sensor_ptr_); + // sensor_ptr_->addProcessor(processor_ptr_); std::cout << "sensor & processor created and added to wolf problem" << std::endl; diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index 3d9e5b49710a17fcfa26d7055a3ff4e50b4fed23..a6de1caf9f99b83539b91ea5f4ed74916d354cec 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -54,16 +54,6 @@ void FeatureBase::remove() FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr) { factor_list_.push_back(_co_ptr); - _co_ptr->setFeaturePtr(shared_from_this()); - _co_ptr->setProblem(getProblem()); - // add factor to be added in solver - if (getProblem() != nullptr) - { - if (_co_ptr->getStatus() == FAC_ACTIVE) - getProblem()->addFactor(_co_ptr); - } - else - WOLF_TRACE("WARNING: ADDING CONSTRAINT ", _co_ptr->id(), " TO FEATURE ", this->id(), " NOT CONNECTED WITH PROBLEM."); return _co_ptr; } @@ -148,7 +138,7 @@ Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) con std::cout << "Linking FeatureBase" << std::endl; _cpt_ptr->addFeature(shared_from_this()); this->setCapturePtr(_cpt_ptr); - this->setProblem(getProblem()); + this->setProblem(_cpt_ptr->getProblem()); } } // namespace wolf diff --git a/src/frame_base.cpp b/src/frame_base.cpp index aea78b7444c4478897affc1dc7af0dd099edd70c..cab2798a0106188e55e243eb5babb530fb3828ea 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -279,9 +279,6 @@ FrameBasePtr FrameBase::getNextFrame() const CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr) { capture_list_.push_back(_capt_ptr); - _capt_ptr->setFramePtr(shared_from_this()); - _capt_ptr->setProblem(getProblem()); - _capt_ptr->registerNewStateBlocks(); return _capt_ptr; } diff --git a/src/hardware_base.cpp b/src/hardware_base.cpp index 3349005129e275a5dd930cd0927d408149994680..a9ac36bf4d2c0b1a213348d7f9f58d2db2b2b1f8 100644 --- a/src/hardware_base.cpp +++ b/src/hardware_base.cpp @@ -17,10 +17,6 @@ HardwareBase::~HardwareBase() SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr) { sensor_list_.push_back(_sensor_ptr); - _sensor_ptr->setProblem(getProblem()); - _sensor_ptr->setHardwarePtr(shared_from_this()); - - _sensor_ptr->registerNewStateBlocks(); return _sensor_ptr; } diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp index b31117566ac69542436e471dd1b1639492680a86..31051f68e2ab9b0869a06a4fedc267e62711e9ae 100644 --- a/src/landmark/landmark_polyline_2D.cpp +++ b/src/landmark/landmark_polyline_2D.cpp @@ -262,25 +262,41 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) // FactorPointToLine2DPtr fac_point_line_ptr = std::static_pointer_cast<FactorPointToLine2D>(fac_ptr); // If landmark point constrained -> new factor + // new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), + // std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), + // fac_point_line_ptr->getProcessor(), + // fac_point_line_ptr->getFeaturePointId(), + // _remain_id, + // fac_point_line_ptr->getLandmarkPointAuxId(), + // fac_point_ptr->getApplyLossFunction(), + // fac_point_line_ptr->getStatus()); + // new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), + // std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), + // fac_point_line_ptr->getProcessor(), + // fac_point_line_ptr->getFeaturePointId(), + // fac_point_line_ptr->getLandmarkPointId(), + // _remain_id, + // fac_point_line_ptr->getApplyLossFunction(), + // fac_point_line_ptr->getStatus()); if (fac_point_line_ptr->getLandmarkPointId() == _remove_id) - new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), - std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), - fac_point_line_ptr->getProcessor(), - fac_point_line_ptr->getFeaturePointId(), - _remain_id, - fac_point_line_ptr->getLandmarkPointAuxId(), - fac_point_ptr->getApplyLossFunction(), - fac_point_line_ptr->getStatus()); + FactorBase::emplace<FactorPointToLine2D>(fac_ptr->getFeature(), std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), + std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), + fac_point_line_ptr->getProcessor(), + fac_point_line_ptr->getFeaturePointId(), + _remain_id, + fac_point_line_ptr->getLandmarkPointAuxId(), + fac_point_ptr->getApplyLossFunction(), + fac_point_line_ptr->getStatus()); // If landmark point is aux point -> new factor else if (fac_point_line_ptr->getLandmarkPointAuxId() == _remove_id) - new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), - std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), - fac_point_line_ptr->getProcessor(), - fac_point_line_ptr->getFeaturePointId(), - fac_point_line_ptr->getLandmarkPointId(), - _remain_id, - fac_point_line_ptr->getApplyLossFunction(), - fac_point_line_ptr->getStatus()); + FactorBase::emplace<FactorPointToLine2D>(fac_ptr->getFeature(), std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()), + std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()), + fac_point_line_ptr->getProcessor(), + fac_point_line_ptr->getFeaturePointId(), + fac_point_line_ptr->getLandmarkPointId(), + _remain_id, + fac_point_line_ptr->getApplyLossFunction(), + fac_point_line_ptr->getStatus()); } else throw std::runtime_error ("polyline factor of unknown type"); @@ -292,7 +308,8 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id) std::cout << "deleting factor: " << fac_ptr->id() << std::endl; // add new factor - fac_ptr->getFeature()->addFactor(new_fac_ptr); + // fac_ptr->getFeature()->addFactor(new_fac_ptr); + new_fac_ptr->link(fac_ptr->getFeature()); // remove factor fac_ptr->remove(); diff --git a/src/map_base.cpp b/src/map_base.cpp index fdfbf11fbb0965d1ebab126539cb4e5c27bb3a2c..dc9f6e2fd3eaee2c0e5c474409bb88185b5c696d 100644 --- a/src/map_base.cpp +++ b/src/map_base.cpp @@ -30,9 +30,6 @@ MapBase::~MapBase() LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr) { landmark_list_.push_back(_landmark_ptr); - _landmark_ptr->setMapPtr(shared_from_this()); - _landmark_ptr->setProblem(getProblem()); - _landmark_ptr->registerNewStateBlocks(); return _landmark_ptr; } diff --git a/src/problem.cpp b/src/problem.cpp index c410886afb7395cb445c11b22287db4216f2b76a..5752f79e3cd991de967f8e78ffc71ef27f93a8af 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -79,7 +79,8 @@ Problem::~Problem() void Problem::addSensor(SensorBasePtr _sen_ptr) { - getHardware()->addSensor(_sen_ptr); + // getHardware()->addSensor(_sen_ptr); + _sen_ptr->link(getHardware()); } SensorBasePtr Problem::installSensor(const std::string& _sen_type, // @@ -123,7 +124,8 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _prc_params, _corresponding_sensor_ptr); prc_ptr->configure(_corresponding_sensor_ptr); - _corresponding_sensor_ptr->addProcessor(prc_ptr); + // _corresponding_sensor_ptr->addProcessor(prc_ptr); + prc_ptr->link(_corresponding_sensor_ptr); // setting the origin in all processor motion if origin already setted if (prc_ptr->isMotion() && prior_is_set_) @@ -216,7 +218,8 @@ FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, // const TimeStamp& _time_stamp) { FrameBasePtr frm = FrameFactory::get().create(_frame_structure, _frame_key_type, _time_stamp, _frame_state); - trajectory_ptr_->addFrame(frm); + // trajectory_ptr_->addFrame(frm); + frm->link(trajectory_ptr_); return frm; } @@ -681,12 +684,16 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen: // create origin capture with the given state as data // Capture fix only takes 3D position and Quaternion orientation CapturePosePtr init_capture; + CaptureBasePtr init_capture_base; + // init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6)); + // init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov); if (trajectory_ptr_->getFrameStructure() == "POV 3D") - init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6)); + init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6)); else - init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov); + init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov); - origin_keyframe->addCapture(init_capture); + init_capture = std::static_pointer_cast<CapturePose>(init_capture_base); + // origin_keyframe->addCapture(init_capture); // emplace feature and factor init_capture->emplaceFeatureAndFactor(); diff --git a/src/processor/processor_GPS.cpp b/src/processor/processor_GPS.cpp index 8409f78c2572489083d36a6c9deb31c45ed53ee1..ecd1dd23bd1fe60ca1499f54db1bd9ab933da31e 100644 --- a/src/processor/processor_GPS.cpp +++ b/src/processor/processor_GPS.cpp @@ -37,14 +37,16 @@ void ProcessorGPS::process(CaptureBasePtr _capture_ptr) { Eigen::Vector3s sat_pos = obs.measurements_[i].sat_position_; Scalar pr = obs.measurements_[i].pseudorange_; - capture_gps_ptr_->addFeature(std::make_shared<FeatureGPSPseudorange>(sat_pos, pr, gps_covariance_)); + // capture_gps_ptr_->addFeature(std::make_shared<FeatureGPSPseudorange>(sat_pos, pr, gps_covariance_)); + FeatureBase::emplace<FeatureGPSPseudorange>(capture_gps_ptr_, sat_pos, pr, gps_covariance_); } //std::cout << "gps features extracted" << std::endl; //std::cout << "Establishing factors to gps features..." << std::endl; for (auto i_it = capture_gps_ptr_->getFeatureList().begin(); i_it != capture_gps_ptr_->getFeatureList().end(); i_it++) { - capture_gps_ptr_->getFeatureList().front()->addFactor(std::make_shared<FactorGPSPseudorange2D>((*i_it), shared_from_this())); + // capture_gps_ptr_->getFeatureList().front()->addFactor(std::make_shared<FactorGPSPseudorange2D>((*i_it), shared_from_this())); + FactorBase::emplace<FactorGPSPseudorange2D>(capture_gps_ptr_->getFeatureList().front(), (*i_it), shared_from_this()); } //std::cout << "Factors established" << std::endl; } diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp index 3f1f5b2445ea42bda8b863beddaf52210bbf4c4c..dde8bc48263a8f492fdfb34535e17af68469c347 100644 --- a/src/processor/processor_IMU.cpp +++ b/src/processor/processor_IMU.cpp @@ -214,12 +214,13 @@ FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, Captur { CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin); FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion); - FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this()); + // FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this()); + auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this()); // link ot wolf tree - _feature_motion->addFactor(fac_imu); - _capture_origin->addConstrainedBy(fac_imu); - _capture_origin->getFrame()->addConstrainedBy(fac_imu); + // _feature_motion->addFactor(fac_imu); + // _capture_origin->addConstrainedBy(fac_imu); + // _capture_origin->getFrame()->addConstrainedBy(fac_imu); return fac_imu; } diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 84923e65bde5b79627e58af16b6990e3f5f2ef1a..115c4fe9e73c755020bf6aa35102020ef734bbff 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -31,7 +31,8 @@ FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _captur std::cout << "Making " << (_type == KEY_FRAME? "key-" : "") << "frame" << std::endl; FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(_type, _capture_ptr->getTimeStamp()); - new_frame_ptr->addCapture(_capture_ptr); + // new_frame_ptr->addCapture(_capture_ptr); + _capture_ptr->link(new_frame_ptr); return new_frame_ptr; } diff --git a/src/processor/processor_capture_holder.cpp b/src/processor/processor_capture_holder.cpp index 2958739d7e6d0c45e8db4d2df31c5c1db2ae5ae0..5ea980e9ac72dad48bc820c37aced78e7cf2a348 100644 --- a/src/processor/processor_capture_holder.cpp +++ b/src/processor/processor_capture_holder.cpp @@ -52,7 +52,8 @@ void ProcessorCaptureHolder::keyFrameCallback(FrameBasePtr _keyframe_ptr, if (frame_ptr != _keyframe_ptr) { - _keyframe_ptr->addCapture(existing_capture); + // _keyframe_ptr->addCapture(existing_capture); + existing_capture->link(_keyframe_ptr); //WOLF_INFO("Adding capture laser !"); diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp index 4e307962e94a3fc2e205cf7cd1c80f480d2871c4..f6fc883f8f5a96ac2c213ab88fc0fdb5c76be8c8 100644 --- a/src/processor/processor_diff_drive.cpp +++ b/src/processor/processor_diff_drive.cpp @@ -212,12 +212,13 @@ CaptureMotionPtr ProcessorDiffDrive::createCapture(const TimeStamp& _ts, FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin) { - FactorOdom2DPtr fac_odom = - std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(), - shared_from_this()); - - _feature->addFactor(fac_odom); - _capture_origin->getFrame()->addConstrainedBy(fac_odom); + // FactorOdom2DPtr fac_odom = + // std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(), + // shared_from_this()); + auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _feature, _capture_origin->getFrame(), + shared_from_this()); + // _feature->addFactor(fac_odom); + // _capture_origin->getFrame()->addConstrainedBy(fac_odom); return fac_odom; } diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 4798ead84ec1c9ebdf1bea82bd2155bf9d17f454..0004da85f26d9e0b74fae44e19645a8ecf8702ea 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -571,14 +571,16 @@ CaptureMotionPtr ProcessorMotion::emplaceCapture(const FrameBasePtr& _frame_own, capture->setCalibrationPreint(_calib_preint); // add to wolf tree - _frame_own->addCapture(capture); + // _frame_own->addCapture(capture); + capture->link(_frame_own); return capture; } FeatureBasePtr ProcessorMotion::emplaceFeature(CaptureMotionPtr _capture_motion) { FeatureBasePtr feature = createFeature(_capture_motion); - _capture_motion->addFeature(feature); + // _capture_motion->addFeature(feature); + feature->link(_capture_motion); return feature; } diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp index cf10be08a27ce9e9af902b86a2117a4bfbfa6c2f..8d6878daf68c9a2b9ff4ea33a7e8b35aed46ea5e 100644 --- a/src/processor/processor_odom_2D.cpp +++ b/src/processor/processor_odom_2D.cpp @@ -154,10 +154,12 @@ CaptureMotionPtr ProcessorOdom2D::createCapture(const TimeStamp& _ts, const Sens FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin) { - FactorOdom2DPtr fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(), - shared_from_this()); - _feature->addFactor(fac_odom); - _capture_origin->getFrame()->addConstrainedBy(fac_odom); + // FactorOdom2DPtr fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(), + // shared_from_this()); + auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _feature, _capture_origin->getFrame(), + shared_from_this()); + // _feature->addFactor(fac_odom); + // _capture_origin->getFrame()->addConstrainedBy(fac_odom); return fac_odom; } @@ -189,13 +191,6 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const return prc_ptr; } -void ProcessorOdom2D::link(SensorBasePtr _sen_ptr) -{ - std::cout << "Linking ProcessorOdom2D" << std::endl; - _sen_ptr->addProcessor(shared_from_this()); - this->setSensorPtr(_sen_ptr); -} - } // Register in the ProcessorFactory diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp index 7415459711b62482df8a0de5d32bb56a496180d2..c2b03c307b0a5eaf970a55dbe0d6d4bdac2e5ee7 100644 --- a/src/processor/processor_odom_3D.cpp +++ b/src/processor/processor_odom_3D.cpp @@ -405,10 +405,12 @@ CaptureMotionPtr ProcessorOdom3D::createCapture(const TimeStamp& _ts, const Sens FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) { - FactorOdom3DPtr fac_odom = std::make_shared<FactorOdom3D>(_feature_motion, _capture_origin->getFrame(), - shared_from_this()); - _feature_motion->addFactor(fac_odom); - _capture_origin->getFrame()->addConstrainedBy(fac_odom); + // FactorOdom3DPtr fac_odom = std::make_shared<FactorOdom3D>(_feature_motion, _capture_origin->getFrame(), + // shared_from_this()); + auto fac_odom = FactorBase::emplace<FactorOdom3D>(_feature_motion, _feature_motion, _capture_origin->getFrame(), + shared_from_this()); + // _feature_motion->addFactor(fac_odom); + // _capture_origin->getFrame()->addConstrainedBy(fac_odom); return fac_odom; } diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index e502ba296a1a4fee6a355839c9d7a6d52871a54a..9693861d3630bbda063a2283bd899277344ea556 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -58,7 +58,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() ); // Append incoming to KF - pack->key_frame->addCapture(incoming_ptr_); + // pack->key_frame->addCapture(incoming_ptr_); + incoming_ptr_->link(pack->key_frame); // Process info // TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do. @@ -76,7 +77,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) case FIRST_TIME_WITHOUT_PACK : { FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY_FRAME, incoming_ptr_->getTimeStamp()); - kfrm->addCapture(incoming_ptr_); + // kfrm->addCapture(incoming_ptr_); + incoming_ptr_->link(kfrm); // Process info processKnown(); @@ -102,7 +104,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) case SECOND_TIME_WITHOUT_PACK : { FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp()); - frm->addCapture(incoming_ptr_); + // frm->addCapture(incoming_ptr_); + incoming_ptr_->link(frm); // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF. @@ -134,11 +137,14 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) FrameBasePtr last_old_frame = last_ptr_->getFrame(); last_old_frame->unlinkCapture(last_ptr_); last_old_frame->remove(); - pack->key_frame->addCapture(last_ptr_); + // pack->key_frame->addCapture(last_ptr_); + last_ptr_->link(pack->key_frame); // Create new frame FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp()); - frm->addCapture(incoming_ptr_); + // frm->addCapture(incoming_ptr_); + incoming_ptr_->link(frm); + // Detect new Features, initialize Landmarks, create Factors, ... processNew(params_tracker_->max_new_features); @@ -174,7 +180,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) // make F; append incoming to new F FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp()); - frm->addCapture(incoming_ptr_); + // frm->addCapture(incoming_ptr_); + incoming_ptr_->link(frm); // process processNew(params_tracker_->max_new_features); @@ -203,7 +210,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) // We do not create a KF // Advance this - last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame + // last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame + incoming_ptr_->link(last_ptr_->getFrame()); last_ptr_->remove(); incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp()); diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp index 670f3aaf776d371bc1db000d84184f5643cc8e2e..4026f4324d08231190e098ea2d94d6a5803df360 100644 --- a/src/processor/processor_tracker_feature.cpp +++ b/src/processor/processor_tracker_feature.cpp @@ -148,19 +148,21 @@ void ProcessorTrackerFeature::establishFactors() FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first; FeatureBasePtr feature_in_last = pair_trkid_pair.second.second; - auto fac_ptr = createFactor(feature_in_last, feature_in_origin); - feature_in_last ->addFactor(fac_ptr); - feature_in_origin->addConstrainedBy(fac_ptr); - if (fac_ptr != nullptr) // factor links - { - FrameBasePtr frm = fac_ptr->getFrameOther(); - if (frm) - frm->addConstrainedBy(fac_ptr); - CaptureBasePtr cap = fac_ptr->getCaptureOther(); - if (cap) - cap->addConstrainedBy(fac_ptr); - } + auto fac_ptr = createFactor(feature_in_last, feature_in_origin); + // feature_in_last ->addFactor(fac_ptr); + fac_ptr->link(feature_in_last); + // feature_in_origin->addConstrainedBy(fac_ptr); + + // if (fac_ptr != nullptr) // factor links + // { + // FrameBasePtr frm = fac_ptr->getFrameOther(); + // if (frm) + // frm->addConstrainedBy(fac_ptr); + // CaptureBasePtr cap = fac_ptr->getCaptureOther(); + // if (cap) + // cap->addConstrainedBy(fac_ptr); + // } WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(), diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp index 4414d8caf499e187e28961b54f8a65e80455716a..c68ea29713f8977c86f0621852d767c846e1a500 100644 --- a/src/processor/processor_tracker_feature_corner.cpp +++ b/src/processor/processor_tracker_feature_corner.cpp @@ -132,7 +132,8 @@ FactorBasePtr ProcessorTrackerFeatureCorner::createFactor(FeatureBasePtr _featur _feature_ptr->getMeasurement()(3)); // Add landmark factor to the other feature - _feature_other_ptr->addFactor(std::make_shared<FactorCorner2D>(_feature_other_ptr, landmark_ptr, shared_from_this())); + // _feature_other_ptr->addFactor(std::make_shared<FactorCorner2D>(_feature_other_ptr, landmark_ptr, shared_from_this())); + FactorBase::emplace<FactorCorner2D>(_feature_other_ptr, _feature_other_ptr, landmark_ptr, shared_from_this()); } // std::cout << "creating factor: last feature " << _feature_ptr->getMeasurement() diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp index 980c652af00ecb490250ac0baabb375ddf5ba98e..be79d8c9a60ca4b8670e1309498fed670b9b5b88 100644 --- a/src/processor/processor_tracker_feature_trifocal.cpp +++ b/src/processor/processor_tracker_feature_trifocal.cpp @@ -416,12 +416,13 @@ void ProcessorTrackerFeatureTrifocal::establishFactors() FeatureBasePtr ftr_last = pair_trkid_match.second.second; // make factor - FactorAutodiffTrifocalPtr ctr = std::make_shared<FactorAutodiffTrifocal>(ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, FAC_ACTIVE); + // FactorAutodiffTrifocalPtr ctr = std::make_shared<FactorAutodiffTrifocal>(ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, FAC_ACTIVE); + auto ctr = FactorBase::emplace<FactorAutodiffTrifocal>(ftr_last, ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, FAC_ACTIVE); // link to wolf tree - ftr_last->addFactor(ctr); - ftr_orig->addConstrainedBy(ctr); - ftr_prev->addConstrainedBy(ctr); + // ftr_last->addFactor(ctr); + // ftr_orig->addConstrainedBy(ctr); + // ftr_prev->addConstrainedBy(ctr); } } } diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp index 3d5b75c8fcc6e9ef70ef396c31a6960a5aa3a016..ba2bf986ef9065e0568ca630299c10728d7ffc07 100644 --- a/src/processor/processor_tracker_landmark.cpp +++ b/src/processor/processor_tracker_landmark.cpp @@ -135,14 +135,15 @@ void ProcessorTrackerLandmark::establishFactors() lmk); if (fac_ptr != nullptr) // factor links { - last_feature->addFactor(fac_ptr); - lmk->addConstrainedBy(fac_ptr); - FrameBasePtr frm = fac_ptr->getFrameOther(); - if (frm) - frm->addConstrainedBy(fac_ptr); - CaptureBasePtr cap = fac_ptr->getCaptureOther(); - if (cap) - cap->addConstrainedBy(fac_ptr); + // last_feature->addFactor(fac_ptr); + fac_ptr->link(last_feature); + // lmk->addConstrainedBy(fac_ptr); + // FrameBasePtr frm = fac_ptr->getFrameOther(); + // if (frm) + // frm->addConstrainedBy(fac_ptr); + // CaptureBasePtr cap = fac_ptr->getCaptureOther(); + // if (cap) + // cap->addConstrainedBy(fac_ptr); } } } diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp index a8037d037e0cad2f3739a4c1766ae86dcdeccc89..e39e4633e731b06dd7d003e27c92cd1e54f9e4fd 100644 --- a/src/processor/processor_tracker_landmark_polyline.cpp +++ b/src/processor/processor_tracker_landmark_polyline.cpp @@ -813,8 +813,10 @@ void ProcessorTrackerLandmarkPolyline::establishFactors() if (lmk_next_point_id > polyline_landmark->getLastId()) lmk_next_point_id -= polyline_landmark->getNPoints(); //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_next_point_id << std::endl; - last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(), - ftr_point_id, lmk_point_id, lmk_next_point_id)); + // last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(), + // ftr_point_id, lmk_point_id, lmk_next_point_id)); + FactorBase::emplace<FactorPointToLine2D>(last_feature, polyline_feature, polyline_landmark, shared_from_this(), + ftr_point_id, lmk_point_id, lmk_next_point_id); //std::cout << "factor added" << std::endl; } @@ -826,8 +828,10 @@ void ProcessorTrackerLandmarkPolyline::establishFactors() if (lmk_prev_point_id < polyline_landmark->getFirstId()) lmk_prev_point_id += polyline_landmark->getNPoints(); //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl; - last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(), - ftr_point_id, lmk_point_id, lmk_prev_point_id)); + // last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(), + // ftr_point_id, lmk_point_id, lmk_prev_point_id)); + FactorBase::emplace<FactorPointToLine2D>(last_feature, polyline_feature, polyline_landmark, shared_from_this(), + ftr_point_id, lmk_point_id, lmk_prev_point_id); //std::cout << "factor added" << std::endl; } @@ -839,8 +843,10 @@ void ProcessorTrackerLandmarkPolyline::establishFactors() //std::cout << "landmark first id:" << polyline_landmark->getFirstId() << std::endl; //std::cout << "landmark last id:" << polyline_landmark->getLastId() << std::endl; //std::cout << "landmark n points:" << polyline_landmark->getNPoints() << std::endl; - last_feature->addFactor(std::make_shared<FactorPoint2D>(polyline_feature, polyline_landmark, shared_from_this(), - ftr_point_id, lmk_point_id)); + // last_feature->addFactor(std::make_shared<FactorPoint2D>(polyline_feature, polyline_landmark, shared_from_this(), + // ftr_point_id, lmk_point_id)); + FactorBase::emplace<FactorPoint2D>(last_feature, polyline_feature, polyline_landmark, shared_from_this(), + ftr_point_id, lmk_point_id); //std::cout << "factor added" << std::endl; } } diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index bd9cfe28031895a1da00c1574a64e9488035f2ae..55a5c8446248e61d2059d4a23600534a3d59fa98 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -177,10 +177,12 @@ void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& ftr_prior->setProblem(getProblem()); // create & add factor absolute + // ftr_prior->addFactor(std::make_shared<FactorQuaternionAbsolute>(_sb)); + // ftr_prior->addFactor(std::make_shared<FactorBlockAbsolute>(_sb, _start_idx, _size)); if (is_quaternion) - ftr_prior->addFactor(std::make_shared<FactorQuaternionAbsolute>(_sb)); + FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, _sb); else - ftr_prior->addFactor(std::make_shared<FactorBlockAbsolute>(_sb, _start_idx, _size)); + FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, _sb, _start_idx, _size); // store feature in params_prior_map_ params_prior_map_[_i] = ftr_prior; @@ -334,8 +336,6 @@ bool SensorBase::process(const CaptureBasePtr capture_ptr) ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr) { processor_list_.push_back(_proc_ptr); - _proc_ptr->setSensorPtr(shared_from_this()); - _proc_ptr->setProblem(getProblem()); return _proc_ptr; } @@ -409,6 +409,8 @@ void SensorBase::link(HardwareBasePtr _hw_ptr) std::cout << "Linking SensorBase" << std::endl; this->setHardwarePtr(_hw_ptr); this->getHardware()->addSensor(shared_from_this()); + this->setProblem(_hw_ptr->getProblem()); + this->registerNewStateBlocks(); } } // namespace wolf diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp index cf01f2d788ed6a28390a460ee526e566412c9bb6..04bbce792fce08bb662a998c3e2d89a1b0122c3e 100644 --- a/src/trajectory_base.cpp +++ b/src/trajectory_base.cpp @@ -18,13 +18,8 @@ TrajectoryBase::~TrajectoryBase() FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr) { - // link up - _frame_ptr->setTrajectoryPtr(shared_from_this()); - _frame_ptr->setProblem(getProblem()); - if (_frame_ptr->isKey()) { - _frame_ptr->registerNewStateBlocks(); if (last_key_frame_ptr_ == nullptr || last_key_frame_ptr_->getTimeStamp() < _frame_ptr->getTimeStamp()) last_key_frame_ptr_ = _frame_ptr; diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp index b6f183675277a9638a85189dbafc5940578a1c84..1d4d6b866eab77300671b0b3fa827372e613b35d 100644 --- a/test/gtest_capture_base.cpp +++ b/test/gtest_capture_base.cpp @@ -76,7 +76,8 @@ TEST(CaptureBase, Dynamic_sensor_params) TEST(CaptureBase, addFeature) { CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2 - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity())); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity())); + auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2s::Zero(), Matrix2s::Identity()); ASSERT_FALSE(C->getFeatureList().empty()); ASSERT_EQ(C->getFeatureList().front(), f); } @@ -84,7 +85,8 @@ TEST(CaptureBase, addFeature) TEST(CaptureBase, addFeatureList) { CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2 - FeatureBasePtr f_first = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity())); + // FeatureBasePtr f_first = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity())); + auto f_first = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2s::Zero(), Matrix2s::Identity()); ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1); // make a list to add diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp index 3f92fc0872e1f1869a998c0acda2fffaad25f3e3..773e5ce58adc2de0df639569d3c6bc9c847df182 100644 --- a/test/gtest_ceres_manager.cpp +++ b/test/gtest_ceres_manager.cpp @@ -319,9 +319,12 @@ TEST(CeresManager, AddFactor) // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + // CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); + // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f)); // update solver ceres_manager_ptr->update(); @@ -341,9 +344,12 @@ TEST(CeresManager, DoubleAddFactor) // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + // CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); + // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f)); // add factor again P->addFactor(c); @@ -366,9 +372,12 @@ TEST(CeresManager, RemoveFactor) // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + // CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); + // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f)); // update solver ceres_manager_ptr->update(); @@ -394,9 +403,12 @@ TEST(CeresManager, AddRemoveFactor) // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + // CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); + // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f)); ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c); @@ -423,9 +435,12 @@ TEST(CeresManager, DoubleRemoveFactor) // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + // CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); + // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f)); // update solver ceres_manager_ptr->update(); @@ -550,10 +565,14 @@ TEST(CeresManager, FactorsRemoveLocalParam) // Create (and add) 2 factors quaternion FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); - FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); + // CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); + // FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); + FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO())); + // FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); + FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO())); // update solver ceres_manager_ptr->update(); @@ -592,10 +611,14 @@ TEST(CeresManager, FactorsUpdateLocalParam) // Create (and add) 2 factors quaternion FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); - FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); + // CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); + // FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); + FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO())); + // FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); + FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO())); // update solver ceres_manager_ptr->update(); diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp index 6c8990c291af5bb43dc9e042f9ca60dc84854e75..1dd8cbb7d98f579d30e71aeb4306d45a0b38b052 100644 --- a/test/gtest_factor_IMU.cpp +++ b/test/gtest_factor_IMU.cpp @@ -2457,10 +2457,13 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i Eigen::MatrixXs featureFix_cov(6,6); featureFix_cov = Eigen::MatrixXs::Identity(6,6); featureFix_cov(5,5) = 0.1; - CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr)); - FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov)); - FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix))); - + // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr)); + auto capfix = CaptureBase::emplace<CaptureMotion>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr); + // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov)); + auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov); + // FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix))); + FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix)); + //prepare problem for solving origin_KF->getP()->fix(); origin_KF->getO()->unfix(); @@ -2515,10 +2518,13 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_ Eigen::MatrixXs featureFix_cov(6,6); featureFix_cov = Eigen::MatrixXs::Identity(6,6); featureFix_cov(5,5) = 0.1; - CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr)); - FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov)); - FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix))); - + // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr)); + auto capfix = CaptureBase::emplace<CaptureMotion>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr); + // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov)); + auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov); + // FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix))); + FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix)); + //prepare problem for solving origin_KF->getP()->fix(); origin_KF->getO()->unfix(); diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp index 204c6ce633a36380d46eacbc65d66abe1a7747fe..a651aae785de3864892d4f62652ebf5fcfc2c105 100644 --- a/test/gtest_factor_absolute.cpp +++ b/test/gtest_factor_absolute.cpp @@ -38,7 +38,8 @@ CeresManager ceres_mgr(problem_ptr); FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0)); // Capture, feature and factor -CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr)); +// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr)); +auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr); //////////////////////////////////////////////////////// /* In the tests below, we check that FactorBlockAbsolute and FactorQuaternionAbsolute are working fine @@ -48,8 +49,10 @@ CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS" TEST(FactorBlockAbs, fac_block_abs_p) { - FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>())); - fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP())); + // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>())); + auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()); + // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP())); + FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP()); ASSERT_TRUE(problem_ptr->check(0)); @@ -66,8 +69,10 @@ TEST(FactorBlockAbs, fac_block_abs_p) TEST(FactorBlockAbs, fac_block_abs_p_tail2) { - FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>())); - fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2)); + // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>())); + auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>()); + // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2)); + FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(),1,2); // Unfix frame 0, perturb frm0 frm0->unfix(); @@ -82,8 +87,10 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2) TEST(FactorBlockAbs, fac_block_abs_v) { - FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>())); - fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV())); + // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>())); + auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()); + // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV())); + FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getV()); ASSERT_TRUE(problem_ptr->check(0)); @@ -100,8 +107,10 @@ TEST(FactorBlockAbs, fac_block_abs_v) TEST(FactorQuatAbs, fac_block_abs_o) { - FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3))); - fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO())); + // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3))); + auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)); + // fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO())); + FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0->getFrame()->getO()); ASSERT_TRUE(problem_ptr->check(0)); diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp index a053b3f82438b8ac9c358fc45b4b656dd9df65ff..640694b774c8e4febc008454319b95cfb72b5bbf 100644 --- a/test/gtest_factor_autodiff.cpp +++ b/test/gtest_factor_autodiff.cpp @@ -29,17 +29,20 @@ TEST(CaptureAutodiff, ConstructorOdom2d) SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE - CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - fr2_ptr->addCapture(capture_ptr); + // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); + // fr2_ptr->addCapture(capture_ptr); + auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); // FEATURE - FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity()); - capture_ptr->addFeature(feature_ptr); + // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity()); + // capture_ptr->addFeature(feature_ptr); + auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity()); - // CONSTRAINT - FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - feature_ptr->addFactor(factor_ptr); - fr1_ptr->addConstrainedBy(factor_ptr); + // FACTOR + // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); + // feature_ptr->addFactor(factor_ptr); + // fr1_ptr->addConstrainedBy(factor_ptr); + auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr); ASSERT_TRUE(factor_ptr->getFeature()); ASSERT_TRUE(factor_ptr->getFeature()->getCapture()); @@ -64,19 +67,24 @@ TEST(CaptureAutodiff, ResidualOdom2d) SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE - CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - fr2_ptr->addCapture(capture_ptr); + // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); + // fr2_ptr->addCapture(capture_ptr); + auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); + // FEATURE Eigen::Vector3s d; d << -1, -4, M_PI/2; - FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); - capture_ptr->addFeature(feature_ptr); + // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); + // capture_ptr->addFeature(feature_ptr); + auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity()); + + // FACTOR + // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); + // feature_ptr->addFactor(factor_ptr); + // fr1_ptr->addConstrainedBy(factor_ptr); - // CONSTRAINT - FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - feature_ptr->addFactor(factor_ptr); - fr1_ptr->addConstrainedBy(factor_ptr); + auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr); // EVALUATE @@ -111,19 +119,22 @@ TEST(CaptureAutodiff, JacobianOdom2d) SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE - CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - fr2_ptr->addCapture(capture_ptr); + // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); + // fr2_ptr->addCapture(capture_ptr); + auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); // FEATURE Eigen::Vector3s d; d << -1, -4, M_PI/2; - FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); - capture_ptr->addFeature(feature_ptr); + // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); + // capture_ptr->addFeature(feature_ptr); + auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity()); - // CONSTRAINT - FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - feature_ptr->addFactor(factor_ptr); - fr1_ptr->addConstrainedBy(factor_ptr); + // FACTOR + // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); + // feature_ptr->addFactor(factor_ptr); + // fr1_ptr->addConstrainedBy(factor_ptr); + auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr); // COMPUTE JACOBIANS @@ -193,22 +204,26 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic) SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE - CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); - fr2_ptr->addCapture(capture_ptr); + // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); + // fr2_ptr->addCapture(capture_ptr); + auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); // FEATURE Eigen::Vector3s d; d << -1, -4, M_PI/2; - FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); - capture_ptr->addFeature(feature_ptr); - - // CONSTRAINTS - FactorOdom2DPtr fac_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); - feature_ptr->addFactor(fac_autodiff_ptr); - fr1_ptr->addConstrainedBy(fac_autodiff_ptr); - FactorOdom2DAnalyticPtr fac_analytic_ptr = std::make_shared<FactorOdom2DAnalytic>(feature_ptr, fr1_ptr); - feature_ptr->addFactor(fac_analytic_ptr); - fr1_ptr->addConstrainedBy(fac_analytic_ptr); + // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity()); + // capture_ptr->addFeature(feature_ptr); + auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity()); + + // FACTOR + // FactorOdom2DPtr fac_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr); + // feature_ptr->addFactor(fac_autodiff_ptr); + // fr1_ptr->addConstrainedBy(fac_autodiff_ptr); + auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr); + // FactorOdom2DAnalyticPtr fac_analytic_ptr = std::make_shared<FactorOdom2DAnalytic>(feature_ptr, fr1_ptr); + // feature_ptr->addFactor(fac_analytic_ptr); + // fr1_ptr->addConstrainedBy(fac_analytic_ptr); + auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2DAnalytic>(feature_ptr, feature_ptr, fr1_ptr); // COMPUTE JACOBIANS diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp index 7559bfa7582aa1449232391806e498fb294d03be..ac4ec5a9cc27bcecc67a368efedfcff1d2245a94 100644 --- a/test/gtest_factor_autodiff_distance_3D.cpp +++ b/test/gtest_factor_autodiff_distance_3D.cpp @@ -58,13 +58,16 @@ class FactorAutodiffDistance3D_Test : public testing::Test F1 = problem->emplaceFrame (KEY_FRAME, pose1, 1.0); F2 = problem->emplaceFrame (KEY_FRAME, pose2, 2.0); - C2 = std::make_shared<CaptureBase>("Base", 1.0); - F2->addCapture(C2); - f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov); - C2->addFeature(f2); - c2 = std::make_shared<FactorAutodiffDistance3D>(f2, F1, nullptr, false, FAC_ACTIVE); - f2->addFactor(c2); - F1->addConstrainedBy(c2); + // C2 = std::make_shared<CaptureBase>("Base", 1.0); + // F2->addCapture(C2); + C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0); + // f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov); + // C2->addFeature(f2); + f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov); + // c2 = std::make_shared<FactorAutodiffDistance3D>(f2, F1, nullptr, false, FAC_ACTIVE); + // f2->addFactor(c2); + // F1->addConstrainedBy(c2); + c2 = std::static_pointer_cast<FactorAutodiffDistance3D>(FactorBase::emplace<FactorAutodiffDistance3D>(f2, f2, F1, nullptr, false, FAC_ACTIVE)); } diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp index 71401381fe35c768d047b8324e0b2e2f74df35c4..496a1ffdba3785a5891f0ad8bbfbb9d73d88da66 100644 --- a/test/gtest_factor_autodiff_trifocal.cpp +++ b/test/gtest_factor_autodiff_trifocal.cpp @@ -701,19 +701,26 @@ class FactorAutodiffTrifocalMultiPointTest : public FactorAutodiffTrifocalTest cv123.push_back(c123); for (size_t i=1; i<c1p_can.cols() ; i++) { - fv1.push_back(std::make_shared<FeatureBase>("PIXEL", c1p_can.col(i), pix_cov)); - I1->addFeature(fv1.at(i)); - - fv2.push_back(std::make_shared<FeatureBase>("PIXEL", c2p_can.col(i), pix_cov)); - I2->addFeature(fv2.at(i)); - - fv3.push_back(std::make_shared<FeatureBase>("PIXEL", c3p_can.col(i), pix_cov)); - I3->addFeature(fv3.at(i)); - - cv123.push_back(std::make_shared<FactorAutodiffTrifocal>(fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, FAC_ACTIVE)); - fv3.at(i)->addFactor(cv123.at(i)); - fv1.at(i)->addConstrainedBy(cv123.at(i)); - fv2.at(i)->addConstrainedBy(cv123.at(i)); + // fv1.push_back(std::make_shared<FeatureBase>("PIXEL", c1p_can.col(i), pix_cov)); + auto f = FeatureBase::emplace<FeatureBase>(I1, "PIXEL", c1p_can.col(i), pix_cov); + fv1.push_back(f); + // I1->addFeature(fv1.at(i)); + + // fv2.push_back(std::make_shared<FeatureBase>("PIXEL", c2p_can.col(i), pix_cov)); + auto f2 = FeatureBase::emplace<FeatureBase>(I2, "PIXEL", c2p_can.col(i), pix_cov); + fv2.push_back(f2); + // I2->addFeature(fv2.at(i)); + + // fv3.push_back(std::make_shared<FeatureBase>("PIXEL", c3p_can.col(i), pix_cov)); + auto f3 = FeatureBase::emplace<FeatureBase>(I3, "PIXEL", c3p_can.col(i), pix_cov); + fv3.push_back(f3); + // I3->addFeature(fv3.at(i)); + + auto ff = std::static_pointer_cast<FactorAutodiffTrifocal>(FactorBase::emplace<FactorAutodiffTrifocal>(fv3.at(i), fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, FAC_ACTIVE)); + cv123.push_back(ff); + // fv3.at(i)->addFactor(cv123.at(i)); + // fv1.at(i)->addConstrainedBy(cv123.at(i)); + // fv2.at(i)->addConstrainedBy(cv123.at(i)); } } @@ -884,13 +891,16 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance) Scalar distance = sqrt(2.0); Scalar distance_std = 0.1; - CaptureBasePtr Cd = std::make_shared<CaptureBase>("DISTANCE", F3->getTimeStamp()); - F3->addCapture(Cd); - FeatureBasePtr fd = std::make_shared<FeatureBase>("DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std)); - Cd->addFeature(fd); - FactorAutodiffDistance3DPtr cd = std::make_shared<FactorAutodiffDistance3D>(fd, F1, nullptr, false, FAC_ACTIVE); - fd->addFactor(cd); - F1->addConstrainedBy(cd); + auto Cd = CaptureBase::emplace<CaptureBase>(F3, "DISTANCE", F3->getTimeStamp()); + // CaptureBasePtr Cd = std::make_shared<CaptureBase>("DISTANCE", F3->getTimeStamp()); + // F3->addCapture(Cd); + auto fd = FeatureBase::emplace<FeatureBase>(Cd, "DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std)); + // FeatureBasePtr fd = std::make_shared<FeatureBase>("DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std)); + // Cd->addFeature(fd); + auto cd = FactorBase::emplace<FactorAutodiffDistance3D>(fd, fd, F1, nullptr, false, FAC_ACTIVE); + // FACTORAUTODIFFDISTANCE3DPTR cd = std::make_shared<FactorAutodiffDistance3D>(fd, F1, nullptr, false, FAC_ACTIVE); + // fd->addFactor(cd); + // F1->addConstrainedBy(cd); cd->setStatus(FAC_INACTIVE); std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp index c3f767d56ea04682d2e33651c4e4e1a89f865585..e4c754ddadacc39a5bf81086052ff0426d8d3dfa 100644 --- a/test/gtest_factor_odom_3D.cpp +++ b/test/gtest_factor_odom_3D.cpp @@ -41,10 +41,13 @@ FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState( FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY_FRAME, delta, TimeStamp(1)); // Capture, feature and factor from frm1 to frm0 -CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr)); -FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov)); -FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(fea1->addFactor(std::make_shared<FactorOdom3D>(fea1, frm0, nullptr))); // create and add -FactorBasePtr dummy = frm0->addConstrainedBy(ctr1); +// CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr)); +auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "ODOM 3D", 1, nullptr, delta, 7, 6, nullptr); +// FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov)); +auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "ODOM 3D", delta, data_cov); +// FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(fea1->addFactor(std::make_shared<FactorOdom3D>(fea1, frm0, nullptr))); // create and add +FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(FactorBase::emplace<FactorOdom3D>(fea1, fea1, frm0, nullptr)); // create and add +// FactorBasePtr dummy = frm0->addConstrainedBy(ctr1); //////////////////////////////////////////////////////// diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp index d7bce0c84f734613f1a2f143bebf2690897149f2..fdcd2d624ce5841a10351cb1ff2b7d2e6b6fdf24 100644 --- a/test/gtest_factor_pose_2D.cpp +++ b/test/gtest_factor_pose_2D.cpp @@ -33,9 +33,12 @@ CeresManager ceres_mgr(problem); FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0)); // Capture, feature and factor from frm1 to frm0 -CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr)); -FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov)); -FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0))); +// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr)); +auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 2D", 0, nullptr, pose, 3, 3, nullptr); +// FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov)); +auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 2D", pose, data_cov); +// FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0))); +FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(fea0, fea0)); //////////////////////////////////////////////////////// diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp index 0af4c3c406f02c9c87cabb35b5e43fd7137fe28b..ab7f30ecf774254f715ee64a034949ec9dfb7fa7 100644 --- a/test/gtest_factor_pose_3D.cpp +++ b/test/gtest_factor_pose_3D.cpp @@ -39,9 +39,12 @@ CeresManager ceres_mgr(problem); FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0)); // Capture, feature and factor -CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr)); -FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov)); -FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(fea0->addFactor(std::make_shared<FactorPose3D>(fea0))); +// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr)); +auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr); +// FEATUREBASEPTR fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov)); +auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 3D", pose7, data_cov); +// FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(fea0->addFactor(std::make_shared<FactorPose3D>(fea0))); +FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(fea0, fea0)); //////////////////////////////////////////////////////// diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp index 2e8245e1544c1a51e280a0874a24d400527a1466..6ba19ec9cec9d468625f3178d6877bfd9e160f5c 100644 --- a/test/gtest_feature_IMU.cpp +++ b/test/gtest_feature_IMU.cpp @@ -77,8 +77,9 @@ class FeatureIMU_test : public testing::Test //create Frame ts = problem->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = problem->getProcessorMotion()->getCurrentState(); - last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3))); - problem->getTrajectory()->addFrame(last_frame); + // last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3))); + last_frame = FrameBase::emplace<FrameBase>(problem->getTrajectory(), KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3))); + // problem->getTrajectory()->addFrame(last_frame); //create a feature delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_; diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index 06436282a934f4b61bcfe04e50c77b1f3f27eea8..41334dfe8ae0f1e4f97648cff841e3076308fff5 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -67,29 +67,37 @@ TEST(FrameBase, LinksToTree) IntrinsicsOdom2D intrinsics_odo; intrinsics_odo.k_disp_to_disp = 1; intrinsics_odo.k_rot_to_rot = 1; - SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); - P->getHardware()->addSensor(S); - FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - T->addFrame(F1); - FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - T->addFrame(F2); - CaptureMotionPtr C = make_shared<CaptureMotion>("MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr); - F1->addCapture(C); + // SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); + // P->getHardware()->addSensor(S); + auto S = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Vector3s::Zero(), intrinsics_odo); + // FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + // T->addFrame(F1); + auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + // FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + // T->addFrame(F2); + auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + // CaptureMotionPtr C = make_shared<CaptureMotion>("MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr); + // F1->addCapture(C); + auto C = CaptureBase::emplace<CaptureMotion>(F1, "MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr); /// @todo link sensor & proccessor ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>(make_shared<ProcessorParamsOdom2D>()); - FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01); - C->addFeature(f); - FactorOdom2DPtr c = make_shared<FactorOdom2D>(f, F2, p); - f->addFactor(c); - + // FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01); + // C->addFeature(f); + auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01); + // FactorOdom2DPtr c = make_shared<FactorOdom2D>(f, F2, p); + // f->addFactor(c); + auto c = FactorBase::emplace<FactorOdom2D>(f, f, F2, p); + + //TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic. // c-by link F2 -> c not yet established - ASSERT_TRUE(F2->getConstrainedByList().empty()); + // ASSERT_TRUE(F2->getConstrainedByList().empty()); + ASSERT_FALSE(F2->getConstrainedByList().empty()); // tree is inconsistent since we are missing the constrained_by link - ASSERT_FALSE(P->check(0)); + // ASSERT_FALSE(P->check(0)); // establish constrained_by link F2 -> c - F2->addConstrainedBy(c); + // F2->addConstrainedBy(c); // tree is now consistent ASSERT_TRUE(P->check(0)); diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp index 5f479b19ec09a04c604aaca010c98cfd6bd37c6f..78f5ee81cb8b882640b78753c9ebf82c8015b3e5 100644 --- a/test/gtest_odom_2D.cpp +++ b/test/gtest_odom_2D.cpp @@ -130,18 +130,24 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D) // KF1 and motion from KF0 t += dt; FrameBasePtr F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t); - CaptureBasePtr C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t)); - FeatureBasePtr f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov)); - FactorBasePtr c1 = f1->addFactor(std::make_shared<FactorOdom2D>(f1, F0, nullptr)); - F0->addConstrainedBy(c1); + // CaptureBasePtr C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t)); + auto C1 = CaptureBase::emplace<CaptureBase>(F1, "ODOM 2D", t); + // FeatureBasePtr f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov)); + auto f1 = FeatureBase::emplace<FeatureBase>(C1, "ODOM 2D", delta, delta_cov); + // FactorBasePtr c1 = f1->addFactor(std::make_shared<FactorOdom2D>(f1, F0, nullptr)); + // F0->addConstrainedBy(c1); + auto c1 = FactorBase::emplace<FactorOdom2D>(f1, f1, F0, nullptr); // KF2 and motion from KF1 t += dt; FrameBasePtr F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t); - CaptureBasePtr C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t)); - FeatureBasePtr f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov)); - FactorBasePtr c2 = f2->addFactor(std::make_shared<FactorOdom2D>(f2, F1, nullptr)); - F1->addConstrainedBy(c2); + // CaptureBasePtr C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t)); + auto C2 = CaptureBase::emplace<CaptureBase>(F2, "ODOM 2D", t); + // FeatureBasePtr f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov)); + auto f2 = FeatureBase::emplace<FeatureBase>(C2, "ODOM 2D", delta, delta_cov); + // FactorBasePtr c2 = f2->addFactor(std::make_shared<FactorOdom2D>(f2, F1, nullptr)); + // F1->addConstrainedBy(c2); + auto c2 = FactorBase::emplace<FactorOdom2D>(f2, f2, F1, nullptr); ASSERT_TRUE(Pr->check(0)); diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 8ee7623dc68b7f637677fd24ceb6f90b083a4b39..09d758e651873862fbfd746dbf3cb351dceb8e5f 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -37,8 +37,9 @@ TEST(Problem, Sensors) ProblemPtr P = Problem::create("POV 3D"); // add a dummy sensor - SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false); - P->addSensor(S); + // SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false); + // P->addSensor(S); + auto S = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false); // check pointers ASSERT_EQ(P, S->getProblem()); @@ -54,12 +55,14 @@ TEST(Problem, Processor) ASSERT_FALSE(P->getProcessorMotion()); // add a motion sensor and processor - SensorBasePtr Sm = std::make_shared<SensorOdom3D>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // with dummy intrinsics - P->addSensor(Sm); + // SensorBasePtr Sm = std::make_shared<SensorOdom3D>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // with dummy intrinsics + // P->addSensor(Sm); + auto Sm = SensorBase::emplace<SensorOdom3D>(P->getHardware(), (Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // add motion processor - ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>(std::make_shared<ProcessorParamsOdom3D>()); - Sm->addProcessor(Pm); + // ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>(std::make_shared<ProcessorParamsOdom3D>()); + // Sm->addProcessor(Pm); + auto Pm = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>())); // check motion processor IS NOT set by addSensor <-- using InstallProcessor it should, see test Installers ASSERT_FALSE(P->getProcessorMotion()); @@ -83,8 +86,9 @@ TEST(Problem, Installers) params->time_tolerance = 0.1; params->max_new_features = 5; params->min_features_for_keyframe = 10; - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params)); - S->addProcessor(pt); + // ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params)); + auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(S, ProcessorTrackerFeatureDummy(params)); + // S->addProcessor(pt); // check motion processor IS NOT set ASSERT_FALSE(P->getProcessorMotion()); @@ -219,9 +223,9 @@ TEST(Problem, StateBlocks) params->time_tolerance = 0.1; params->max_new_features = 5; params->min_features_for_keyframe = 10; - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params)); - - St->addProcessor(pt); + // ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params)); + auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params)); + // St->addProcessor(pt); ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); // 2 state blocks, estimated @@ -253,9 +257,10 @@ TEST(Problem, Covariances) params->time_tolerance = 0.1; params->max_new_features = 5; params->min_features_for_keyframe = 10; - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params)); + // ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params)); + auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params)); - St->addProcessor(pt); + // St->addProcessor(pt); ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); // 4 state blocks, estimated diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 7479ca984eda6dff7504595c66a959ae1d3aebfa..754229c69d1dc25cede623abcc5f66528ceb75ea 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -38,18 +38,22 @@ TEST(ProcessorBase, KeyFrameCallback) ProblemPtr problem = Problem::create("PO 2D"); // Install tracker (sensor and processor) - SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), - std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), - std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); + // SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), + // std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), + // std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); + auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(), "FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), + std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), + std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); params->time_tolerance = dt/2; params->max_new_features = 5; params->min_features_for_keyframe = 5; - shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(params); + // shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(params); + auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, params); - problem->addSensor(sens_trk); - sens_trk->addProcessor(proc_trk); + // problem->addSensor(sens_trk); + // sens_trk->addProcessor(proc_trk); // Install odometer (sensor and processor) SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), ""); diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp index a3e18e59c74962936a906681143f9003fa9d99f0..b37be3fa117325cf8f9b5c227ca44a5e5a62b01b 100644 --- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp +++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp @@ -72,23 +72,39 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated) auto stateblock_optr4 = std::make_shared<wolf::StateBlock>(state4.tail<1>()); // create Keyframes - F1 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, - 1, - stateblock_pptr1, - stateblock_optr1); - F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, - 1, - stateblock_pptr2, - stateblock_optr2); - F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, - 1, - stateblock_pptr3, - stateblock_optr3); - F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, - 1, - stateblock_pptr4, - stateblock_optr4); - + // F1 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + // 1, + // stateblock_pptr1, + // stateblock_optr1); + // F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + // 1, + // stateblock_pptr2, + // stateblock_optr2); + // F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + // 1, + // stateblock_pptr3, + // stateblock_optr3); + // F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + // 1, + // stateblock_pptr4, + // stateblock_optr4); + + F1 = wolf::FrameBase::emplace<wolf::FrameBase>(problem->getTrajectory(),wolf::KEY_FRAME, + 1, + stateblock_pptr1, + stateblock_optr1); + F2 = wolf::FrameBase::emplace<wolf::FrameBase>(problem->getTrajectory(),wolf::KEY_FRAME, + 1, + stateblock_pptr2, + stateblock_optr2); + F3 = wolf::FrameBase::emplace<wolf::FrameBase>(problem->getTrajectory(),wolf::KEY_FRAME, + 1, + stateblock_pptr3, + stateblock_optr3); + F4 = wolf::FrameBase::emplace<wolf::FrameBase>(problem->getTrajectory(),wolf::KEY_FRAME, + 1, + stateblock_pptr4, + stateblock_optr4); // add dummy capture capture_dummy = std::make_shared<wolf::CaptureBase>("DUMMY", 1.0, @@ -99,10 +115,10 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated) F4->addCapture(capture_dummy); // Add first three states to trajectory - problem->getTrajectory()->addFrame(F1); - problem->getTrajectory()->addFrame(F2); - problem->getTrajectory()->addFrame(F3); - problem->getTrajectory()->addFrame(F4); + // problem->getTrajectory()->addFrame(F1); + // problem->getTrajectory()->addFrame(F2); + // problem->getTrajectory()->addFrame(F3); + // problem->getTrajectory()->addFrame(F4); // Add same covariances for all states Eigen::Matrix2s position_covariance_matrix; @@ -216,19 +232,21 @@ int main(int argc, char **argv) processor_params_point2d->buffer_size_ = 0; // just exclude identical frameIDs processor_params_point2d->distance_type_ = wolf::LoopclosureDistanceType::LC_POINT_ELLIPSE; - processor_ptr_point2d = std::make_shared<DummyLoopCloser>(processor_params_point2d); + // processor_ptr_point2d = std::make_shared<DummyLoopCloser>(processor_params_point2d); + processor_ptr_point2d = std::static_pointer_cast<wolf::ProcessorFrameNearestNeighborFilter>(wolf::ProcessorBase::emplace<DummyLoopCloser>(sensor_ptr, processor_params_point2d)); processor_ptr_point2d->setName("processor2Dpoint"); - sensor_ptr->addProcessor(processor_ptr_point2d); + // sensor_ptr->addProcessor(processor_ptr_point2d); processor_params_ellipse2d->probability_ = 0.95; processor_params_ellipse2d->buffer_size_ = 0; // just exclude identical frameIDs processor_params_ellipse2d->distance_type_ = wolf::LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE; - processor_ptr_ellipse2d = std::make_shared<DummyLoopCloser>(processor_params_ellipse2d); + // processor_ptr_ellipse2d = std::make_shared<DummyLoopCloser>(processor_params_ellipse2d); + processor_ptr_ellipse2d = std::static_pointer_cast<wolf::ProcessorFrameNearestNeighborFilter>(wolf::ProcessorBase::emplace<DummyLoopCloser>(sensor_ptr, processor_params_ellipse2d)); processor_ptr_ellipse2d->setName("processor2Dellipse"); - sensor_ptr->addProcessor(processor_ptr_ellipse2d); + // sensor_ptr->addProcessor(processor_ptr_ellipse2d); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp index 6819cba18e0ec605a6272e8ac6e05e44cf58a6d3..71154fd5fc333a498c9e4c2e4ea9ffe7a30e0918 100644 --- a/test/gtest_processor_tracker_feature_trifocal.cpp +++ b/test/gtest_processor_tracker_feature_trifocal.cpp @@ -77,9 +77,11 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback) IntrinsicsCameraPtr intr = make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML intr->width = 640; intr->height = 480; - SensorCameraPtr sens_trk = make_shared<SensorCamera>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), - intr); + // SensorCameraPtr sens_trk = make_shared<SensorCamera>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), + // intr); + auto sens_trk = SensorBase::emplace<SensorCamera>(problem->getHardware(), (Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), + intr); ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>(); // params_tracker_feature_trifocal->name = "trifocal"; params_tracker_feature_trifocal->pixel_noise_std = 1.0; @@ -93,11 +95,12 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback) params_tracker_feature_trifocal->max_euclidean_distance = 20; params_tracker_feature_trifocal->yaml_file_params_vision_utils = wolf_root + "/src/examples/ACTIVESEARCH.yaml"; - ProcessorTrackerFeatureTrifocalPtr proc_trk = make_shared<ProcessorTrackerFeatureTrifocal>(params_tracker_feature_trifocal); + // ProcessorTrackerFeatureTrifocalPtr proc_trk = make_shared<ProcessorTrackerFeatureTrifocal>(params_tracker_feature_trifocal); + auto proc_trk = std::static_pointer_cast<ProcessorTrackerFeatureTrifocal>(ProcessorBase::emplace<ProcessorTrackerFeatureTrifocal>(sens_trk, params_tracker_feature_trifocal)); proc_trk->configure(sens_trk); - problem->addSensor(sens_trk); - sens_trk->addProcessor(proc_trk); + // problem->addSensor(sens_trk); + // sens_trk->addProcessor(proc_trk); // Install odometer (sensor and processor) IntrinsicsOdom3DPtr params = std::make_shared<IntrinsicsOdom3D>(); @@ -121,7 +124,8 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback) // Track cv::Mat image(intr->height, intr->width, CV_8UC3); // OpenCV cv::Mat(rows, cols) image *= 0; // TODO see if we want to use a real image - CaptureImagePtr capt_trk = make_shared<CaptureImage>(t, sens_trk, image); + SensorCameraPtr sens_trk_cam = std::static_pointer_cast<SensorCamera>(sens_trk); + CaptureImagePtr capt_trk = make_shared<CaptureImage>(t, sens_trk_cam, image); proc_trk->process(capt_trk); for (size_t ii=0; ii<32; ii++ ) @@ -134,7 +138,7 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback) proc_odo->process(capt_odo); // Track - capt_trk = make_shared<CaptureImage>(t, sens_trk, image); + capt_trk = make_shared<CaptureImage>(t, sens_trk_cam, image); proc_trk->process(capt_trk); CaptureBasePtr prev = proc_trk->getPrevOrigin(); diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index b6b5f2b85717df13c864551845bdf6c1725c0b39..be165e8ca382eb6cd5a9e19294f2b6b15efff184 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -506,9 +506,12 @@ TEST(SolverManager, AddFactor) // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + // CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); + // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f)); // update solver solver_manager_ptr->update(); @@ -528,9 +531,12 @@ TEST(SolverManager, RemoveFactor) // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + // CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); + // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f)); // update solver solver_manager_ptr->update(); @@ -556,9 +562,13 @@ TEST(SolverManager, AddRemoveFactor) // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + // CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + + auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); + FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f)); ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c); @@ -585,9 +595,13 @@ TEST(SolverManager, DoubleRemoveFactor) // Create (and add) factor point 2d FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); - CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); - FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); - FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + // CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); + + auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); + auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()); + FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f)); // update solver solver_manager_ptr->update(); diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index d77defcbc697d87cf827a465d1ef99e9221293a6..e70f26c025b28587e99a1b393189f75c9577d7c4 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -69,12 +69,15 @@ TEST(TrajectoryBase, ClosestKeyFrame) // 1 2 3 time stamps // --+-----+-----+---> time - FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, nullptr, nullptr); - FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, nullptr, nullptr); - FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, nullptr, nullptr); - T->addFrame(f1); - T->addFrame(f2); - T->addFrame(f3); + // FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, nullptr, nullptr); + // FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, nullptr, nullptr); + // FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, nullptr, nullptr); + FrameBasePtr f1 = FrameBase::emplace<FrameBase>(T, KEY_FRAME, 1, nullptr, nullptr); + FrameBasePtr f2 = FrameBase::emplace<FrameBase>(T, KEY_FRAME, 2, nullptr, nullptr); + FrameBasePtr f3 = FrameBase::emplace<FrameBase>(T, NON_KEY_FRAME, 3, nullptr, nullptr); + // T->addFrame(f1); + // T->addFrame(f2); + // T->addFrame(f3); FrameBasePtr kf; // closest key-frame queried @@ -112,24 +115,31 @@ TEST(TrajectoryBase, Add_Remove_Frame) FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame + // FrameBasePtr f1 = FrameBase::emplace<FrameBase>(T, KEY_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed + // FrameBasePtr f2 = FrameBase::emplace<FrameBase>(T, KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not + // FrameBasePtr f3 = FrameBase::emplace<FrameBase>(T, NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame + std::cout << __LINE__ << std::endl; // add frames and keyframes - T->addFrame(f1); // KF + // T->addFrame(f1); // KF + f1->link(T); if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1); ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2); ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2); std::cout << __LINE__ << std::endl; - T->addFrame(f2); // KF + // T->addFrame(f2); // KF + f2->link(T); if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2); ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4); ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4); std::cout << __LINE__ << std::endl; - T->addFrame(f3); // F + // T->addFrame(f3); // F + f3->link(T); if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 3); ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4); @@ -194,17 +204,20 @@ TEST(TrajectoryBase, KeyFramesAreSorted) FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame // add frames and keyframes in random order --> keyframes must be sorted after that - T->addFrame(f2); // KF2 + // T->addFrame(f2); // KF2 + f2->link(T); if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getLastFrame() ->id(), f2->id()); ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); - T->addFrame(f3); // F3 + // T->addFrame(f3); // F3 + f3->link(T); if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); - T->addFrame(f1); // KF1 + // T->addFrame(f1); // KF1 + f1->link(T); if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); @@ -215,7 +228,8 @@ TEST(TrajectoryBase, KeyFramesAreSorted) ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id()); FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame - T->addFrame(f4); + // T->addFrame(f4); + f4->link(T); // Trajectory status: // kf1 kf2 kf3 f4 frames // 1 2 3 1.5 time stamps