diff --git a/src/capture_base.cpp b/src/capture_base.cpp index e860b5757b882d5382ba0752b9dbe87d42aa89bd..f307f2bd561e3b30db1a232c45f4792b4a9e44e7 100644 --- a/src/capture_base.cpp +++ b/src/capture_base.cpp @@ -57,7 +57,7 @@ CaptureBase::CaptureBase(const std::string& _type, } updateCalibSize(); - WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s"); +// WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s"); } diff --git a/src/frame_base.cpp b/src/frame_base.cpp index 088e9effe597908d556b1e8a8f8ea48226fe4822..0891b05cd2bda0b47e1226586f65bc3b2d7ad26d 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -23,9 +23,6 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _ state_block_vec_[0] = _p_ptr; state_block_vec_[1] = _o_ptr; state_block_vec_[2] = _v_ptr; - - if ( isKey() ) - registerNewStateBlocks(); } FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : @@ -40,9 +37,6 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr state_block_vec_[0] = _p_ptr; state_block_vec_[1] = _o_ptr; state_block_vec_[2] = _v_ptr; - - if ( isKey() ) - registerNewStateBlocks(); } FrameBase::~FrameBase() diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp index 416eca9058efde6d43073ba1e94c4af493166e65..81351162ead4b06d53f2170e66f6c986855f2f88 100644 --- a/src/hello_wolf/hello_wolf.cpp +++ b/src/hello_wolf/hello_wolf.cpp @@ -1,7 +1,7 @@ /** * \file hello_wolf.cpp * - * Created on: Dec 1, 2017 -- two months exactly after Oct-1st, we still have 4 political prisoners. + * Created on: Dec 1, 2017 -- two months exactly after Oct-1st, we still have 4 political prisoners. * * ### * ## ## @@ -100,7 +100,7 @@ int main() * - Second, using random values * Both solutions must produce the same exact values as in the sketches above. * - * Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line 139) + * Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line 141) * * (c) 2017 Joan Sola @ IRI-CSIC */ @@ -136,8 +136,10 @@ int main() intrinsics_rb->noise_bearing_degrees_std = 1.0; intrinsics_rb->noise_range_metres_std = 0.1; SensorBasePtr sensor_rb = problem->installSensor("RANGE BEARING", "sensor RB", Vector3s(1,1,0), intrinsics_rb); - // NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only) + + // NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable) // sensor_rb->getOPtr()->unfix(); + // sensor_rb->getPPtr()->unfix(); // The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too. // processor Range and Bearing ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>(); @@ -157,6 +159,7 @@ int main() // SET OF EVENTS ======================================================= + std::cout << std::endl; WOLF_TRACE("======== BUILD PROBLEM =======") // We'll do 3 steps of motion and landmark observations. @@ -205,9 +208,9 @@ int main() ranges << sqrt(2.0), 1.0; // see drawing bearings << 3*M_PI/4, M_PI/2; cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); - sensor_rb ->process(cap_rb); + sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2) - problem->print(4,1,1,1); + problem->print(1,0,1,0); // SOLVE ================================================================ @@ -216,7 +219,7 @@ int main() WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======") std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); WOLF_TRACE(report); // should show a very low iteration number (possibly 1) - problem->print(4,1,1,1); + problem->print(1,0,1,0); // PERTURB initial guess WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======") @@ -227,14 +230,16 @@ int main() for (auto kf : problem->getTrajectoryPtr()->getFrameList()) kf->setState(Vector3s::Random() * 0.5); // We perturb A LOT ! for (auto lmk : problem->getMapPtr()->getLandmarkList()) - lmk->getPPtr()->setState(Vector2s::Random()); // We perturb A LOT ! - problem->print(4,1,1,1); + for (auto sb : lmk->getStateBlockVec()) + if (sb && !sb->isFixed()) + sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! + problem->print(1,0,1,0); // SOLVE again WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======") report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!) - problem->print(4,1,1,1); + problem->print(1,0,1,0); // GET COVARIANCES of all states WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") @@ -244,6 +249,10 @@ int main() WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance()); for (auto lmk : problem->getMapPtr()->getLandmarkList()) WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance()); + std::cout << std::endl; + + WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======") + problem->print(4,1,1,1); /* * ============= FIRST COMMENT ON THE RESULTS ================== @@ -260,9 +269,7 @@ int main() * * - Observe that all other KFs and Lmks are correct. * - * - Observe that F4 is not correct. Since it is not a KF, is has not been estimated. - * But this is a no-issue because F4 is just an inner frame used by the odometer processor, - * with no role in the problem itself, nor in the optimization process. + * - Try self-calibrating the sensor orientation by uncommenting line 141 (well, around 141) * */ diff --git a/src/hello_wolf/processor_range_bearing.cpp b/src/hello_wolf/processor_range_bearing.cpp index 0252e6622cdd48acb65ca51ef2c6b1023805b080..5023023b603d5aa160cf84ddb5719b47550e39b2 100644 --- a/src/hello_wolf/processor_range_bearing.cpp +++ b/src/hello_wolf/processor_range_bearing.cpp @@ -23,38 +23,39 @@ ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor void ProcessorRangeBearing::process(CaptureBasePtr _capture) { + // 1. get KF + FrameBasePtr kf(nullptr); if ( !kf_pack_buffer_.empty() ) { - // Select using incoming_ptr + // KeyFrame Callback received KFPackPtr pack = kf_pack_buffer_.selectPack( _capture->getTimeStamp(), params_->time_tolerance ); if (pack!=nullptr) - keyFrameCallback(pack->key_frame,pack->time_tolerance); + kf = pack->key_frame; kf_pack_buffer_.removeUpTo( _capture->getTimeStamp() ); - } - CaptureRangeBearingPtr capture = std::static_pointer_cast<CaptureRangeBearing>(_capture); + assert( kf && "Callback KF is not close enough to _capture!"); + } - // 1. get KF -- we assume a KF is available to hold this _capture (checked in assert below) - auto kf = getProblem()->closestKeyFrameToTimeStamp(capture->getTimeStamp()); - assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!"); + if (!kf) + { + // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below) + kf = getProblem()->closestKeyFrameToTimeStamp(_capture->getTimeStamp()); + assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!"); + } - // 2. create Capture - auto cap = std::make_shared<CaptureRangeBearing>(capture->getTimeStamp(), - getSensorPtr(), - capture->getIds(), - capture->getRanges(), - capture->getBearings()); - kf->addCapture(cap); + // 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); // 3. explore all observations in the capture - for (SizeEigen i = 0; i < capture->getIds().size(); i++) + for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++) { // extract info - int id = capture->getId (i); - Scalar range = capture->getRange (i); - Scalar bearing = capture->getBearing(i); + int id = capture_rb->getId (i); + Scalar range = capture_rb->getRange (i); + Scalar bearing = capture_rb->getBearing(i); // 4. create or recover landmark LandmarkPoint2DPtr lmk; @@ -80,11 +81,11 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) Vector2s rb(range,bearing); auto ftr = std::make_shared<FeatureRangeBearing>(rb, getSensorPtr()->getNoiseCov()); - cap->addFeature(ftr); + capture_rb->addFeature(ftr); // 6. create constraint auto prc = shared_from_this(); - auto ctr = std::make_shared<ConstraintRangeBearing>(cap, + auto ctr = std::make_shared<ConstraintRangeBearing>(capture_rb, lmk, prc, false, @@ -95,16 +96,6 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) } -Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) const -{ - return polar(toSensor(lmk_w)); -} - -Eigen::Vector2s ProcessorRangeBearing::invObserve(Scalar r, Scalar b) const -{ - return fromSensor(rect(r, b)); -} - ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr) { SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sen_ptr); @@ -119,17 +110,26 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name, return prc; } +Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) const +{ + return polar(toSensor(lmk_w)); +} + ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3s& _pose) const { Trf H = Eigen::Translation<Scalar,2>(_pose(0), _pose(1)) * Eigen::Rotation2D<Scalar>(_pose(2)) ; return H; } +ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2s& _position, + const Eigen::Vector1s& _orientation) const +{ + Trf H = Eigen::Translation<Scalar,2>(_position(0), _position(1)) * Eigen::Rotation2D<Scalar>(_orientation(0)) ; + return H; +} + Eigen::Vector2s ProcessorRangeBearing::fromSensor(const Eigen::Vector2s& lmk_s) const { -// Eigen::Vector2s pos_s = getSensorPtr()->getPPtr()->getState(); -// Eigen::Vector1s ori_s = getSensorPtr()->getOPtr()->getState(); -// Trf H_w_r = transform(pos_s, ori_s); Trf H_w_r = transform(getProblem()->getCurrentState()); return H_w_r * H_r_s * lmk_s; } @@ -149,21 +149,14 @@ Eigen::Vector2s ProcessorRangeBearing::polar(const Eigen::Vector2s& rect) const return polar; } -void ProcessorRangeBearing::keyFrameCallback(FrameBasePtr _key_frame, const Scalar& _time_tolerance) -{ - // -} - Eigen::Vector2s ProcessorRangeBearing::rect(Scalar range, Scalar bearing) const { return range * (Vector2s() << cos(bearing), sin(bearing)).finished(); } -ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2s& _position, - const Eigen::Vector1s& _orientation) const +Eigen::Vector2s ProcessorRangeBearing::invObserve(Scalar r, Scalar b) const { - Trf H = Eigen::Translation<Scalar,2>(_position(0), _position(1)) * Eigen::Rotation2D<Scalar>(_orientation(0)) ; - return H; + return fromSensor(rect(r, b)); } } /* namespace wolf */ diff --git a/src/hello_wolf/processor_range_bearing.h b/src/hello_wolf/processor_range_bearing.h index 5a5f1c82fe5f49946a5a212c13bc6068088e9ef0..5d3d2a8dd83b2390c204d2a0d58e9239ca233548 100644 --- a/src/hello_wolf/processor_range_bearing.h +++ b/src/hello_wolf/processor_range_bearing.h @@ -21,7 +21,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsRangeBearing); struct ProcessorParamsRangeBearing : public ProcessorParamsBase { - // Eigen::Vector3s pose0, delta; + // We do not need special parameters, but in case you need they should be defined here. }; @@ -46,17 +46,16 @@ class ProcessorRangeBearing : public ProcessorBase // Implementation of pure virtuals from ProcessorBase virtual void process (CaptureBasePtr _capture) override; virtual bool voteForKeyFrame () override {return false;} - virtual void keyFrameCallback (FrameBasePtr _key_frame, const Scalar& _time_tolerance) override; - - // landmark observation models -- they would be better off in a separate library e.g. range_bearing_tools.h - Eigen::Vector2s observe (const Eigen::Vector2s& lmk_w) const; - Eigen::Vector2s invObserve (Scalar r, Scalar b) const; private: // control variables Trf H_r_s; // transformation matrix, robot to sensor std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far + protected: + // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h + Eigen::Vector2s observe (const Eigen::Vector2s& lmk_w) const; + Eigen::Vector2s invObserve (Scalar r, Scalar b) const; private: // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h Trf transform (const Eigen::Vector3s& _pose) const; diff --git a/src/sensors/sensor_diff_drive.h b/src/sensors/sensor_diff_drive.h index 749772902dcf39e035b318705a4797028c896d5b..925c840e51f93ade1e6517cfd8409befaf851d8f 100644 --- a/src/sensors/sensor_diff_drive.h +++ b/src/sensors/sensor_diff_drive.h @@ -15,7 +15,7 @@ namespace wolf { WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsDiffDrive); -WOLF_STRUCT_PTR_TYPEDEFS(SensorDiffDrive); +WOLF_PTR_TYPEDEFS(SensorDiffDrive); struct IntrinsicsDiffDrive : public IntrinsicsBase { diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp index df3a9f5f7d932c5049ba093c59d3206ab19ddcc8..3dcb41267e8dea55255dd6f2a4d1ca523997dfc1 100644 --- a/src/trajectory_base.cpp +++ b/src/trajectory_base.cpp @@ -89,7 +89,6 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) if ((*frm_rit)->isKey()) { Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts); - std::cout << "dt " << dt << std::endl; if (dt < min_dt) { min_dt = dt;