diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 01e0b7782b8dae156db2966d162192eef8859015..568bd9472541c0f7b559901cc506766d3fc53c79 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -105,7 +105,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks for (auto sb : fr_ptr->getStateBlockVec()) if (sb) for(auto sb2 : fr_ptr->getStateBlockVec()) - if (sb) + if (sb2) { state_block_pairs.emplace_back(sb, sb2); double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2)); diff --git a/src/frame_base.cpp b/src/frame_base.cpp index 088e9effe597908d556b1e8a8f8ea48226fe4822..97cc1744c92f602ff902b5edc63561b104f4eaba 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -14,7 +14,7 @@ unsigned int FrameBase::frame_id_count_ = 0; FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : NodeBase("FRAME", "Base"), trajectory_ptr_(), - state_block_vec_(3), // allow for 6 state blocks by default. Resize in derived constructors if needed. + state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. is_removing_(false), frame_id_(++frame_id_count_), type_(NON_KEY_FRAME), diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp index 416eca9058efde6d43073ba1e94c4af493166e65..66813522eee1ce7da1290b43b0b2eb194c71e095 100644 --- a/src/hello_wolf/hello_wolf.cpp +++ b/src/hello_wolf/hello_wolf.cpp @@ -27,7 +27,7 @@ #include "ceres_wrapper/ceres_manager.h" int main() -{ + { /* * ============= PROBLEM DEFINITION ================== * @@ -117,12 +117,12 @@ int main() // sensor odometer 2D IntrinsicsOdom2DPtr intrinsics_odo = std::make_shared<IntrinsicsOdom2D>(); - intrinsics_odo->k_disp_to_disp = 0.1; - intrinsics_odo->k_rot_to_rot = 0.1; SensorBasePtr sensor_odo = problem->installSensor("ODOM 2D", "sensor odo", Vector3s(0,0,0), intrinsics_odo); // processor odometer 2D ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>(); + params_odo->voting_active = true; + params_odo->time_tolerance = 0.1; params_odo->max_time_span = 999; params_odo->dist_traveled = 0.95; // Will make KFs automatically every 1m displacement params_odo->angle_turned = 999; @@ -133,14 +133,16 @@ int main() // sensor Range and Bearing IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>(); - intrinsics_rb->noise_bearing_degrees_std = 1.0; intrinsics_rb->noise_range_metres_std = 0.1; + intrinsics_rb->noise_bearing_degrees_std = 1.0; SensorBasePtr sensor_rb = problem->installSensor("RANGE BEARING", "sensor RB", Vector3s(1,1,0), intrinsics_rb); + sensor_rb->fix(); // NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only) - // sensor_rb->getOPtr()->unfix(); + sensor_rb->getOPtr()->unfix(); // processor Range and Bearing ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>(); + params_rb->voting_active = false; params_rb->time_tolerance = 0.01; ProcessorBasePtr processor_rb = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb); @@ -164,10 +166,10 @@ int main() // STEP 1 -------------------------------------------------------------- // initialize - TimeStamp t(0.0); + TimeStamp t(0.0); // t : 0.0 Vector3s x(0,0,0); Matrix3s P = Matrix3s::Identity() * 0.1; - problem->setPrior(x, P, t, 0.5); // KF1 + problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0) // observe lmks ids.resize(1); ranges.resize(1); bearings.resize(1); @@ -175,14 +177,14 @@ int main() ranges << 1.0; // see drawing bearings << M_PI/2; CaptureRangeBearingPtr 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) // STEP 2 -------------------------------------------------------------- - t += 1.0; + t += 1.0; // t : 1.0 // motion CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov); - sensor_odo ->process(cap_motion); // KF2 + sensor_odo ->process(cap_motion); // KF2 : (1,0,0) // observe lmks ids.resize(2); ranges.resize(2); bearings.resize(2); @@ -190,14 +192,14 @@ 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) // STEP 3 -------------------------------------------------------------- - t += 1.0; + t += 1.0; // t : 2.0 // motion cap_motion ->setTimeStamp(t); - sensor_odo ->process(cap_motion); // KF3 + sensor_odo ->process(cap_motion); // KF3 : (2,0,0) // observe lmks ids.resize(2); ranges.resize(2); bearings.resize(2); @@ -205,9 +207,7 @@ 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); - - problem->print(4,1,1,1); + sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2) // SOLVE ================================================================ @@ -216,25 +216,30 @@ 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(4,0,1,1); // PERTURB initial guess WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======") for (auto sen : problem->getHardwarePtr()->getSensorList()) for (auto sb : sen->getStateBlockVec()) if (sb && !sb->isFixed()) - sb->setState(VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! + sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! for (auto kf : problem->getTrajectoryPtr()->getFrameList()) - kf->setState(Vector3s::Random() * 0.5); // We perturb A LOT ! + if (kf->isKey()) + for (auto sb : kf->getStateBlockVec()) + if (sb && !sb->isFixed()) + sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 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(2,0,1,1); // 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,1,1,1); // GET COVARIANCES of all states WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")