diff --git a/include/imu/processor/processor_compass.h b/include/imu/processor/processor_compass.h index a3db652bc7efaf9ec94ce86e9fd1daa69c5fb5b7..346d640cd574333ebd525e67e27f5687249aa136 100644 --- a/include/imu/processor/processor_compass.h +++ b/include/imu/processor/processor_compass.h @@ -59,11 +59,11 @@ class ProcessorCompass : public ProcessorBase protected: void processCapture(CaptureBasePtr) override; - void processKeyFrame(FrameBasePtr, const double&) override; + void processKeyFrame(FrameBasePtr _frm) override; void processMatch(FrameBasePtr, CaptureBasePtr); bool triggerInCapture(CaptureBasePtr _cap) const override { return true;}; - bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;}; + bool triggerInKeyFrame(FrameBasePtr _frm) const override { return true;}; bool storeKeyFrame(FrameBasePtr _frm) override { return false;}; bool storeCapture(CaptureBasePtr _cap) override { return false;}; diff --git a/src/processor/processor_compass.cpp b/src/processor/processor_compass.cpp index 956f7581ced4bdf6ef66d8687a5dc999e20242e5..cd7d24ea2d08e4416a6783a992f4ae00c992ea87 100644 --- a/src/processor/processor_compass.cpp +++ b/src/processor/processor_compass.cpp @@ -39,24 +39,24 @@ ProcessorCompass::~ProcessorCompass() void ProcessorCompass::processCapture(CaptureBasePtr _capture) { // Search for any stored frame within time tolerance of capture - auto frame_pack = buffer_pack_kf_.select(_capture->getTimeStamp(), params_->time_tolerance); - if (frame_pack) + auto keyframe = buffer_frame_.select(_capture->getTimeStamp(), params_->time_tolerance); + if (keyframe) { - processMatch(frame_pack->key_frame, _capture); + processMatch(keyframe, _capture); // remove the frame and older frames - buffer_pack_kf_.removeUpTo(frame_pack->key_frame->getTimeStamp()); + buffer_frame_.removeUpTo(keyframe->getTimeStamp()); } // Otherwise: store capture // Note that more than one processor can be emplacing frames, so an older frame can arrive later than this one. else - buffer_capture_.add(_capture->getTimeStamp(), _capture); + buffer_capture_.emplace(_capture->getTimeStamp(), _capture); } -void ProcessorCompass::processKeyFrame(FrameBasePtr _frame, const double& _time_tolerance) +void ProcessorCompass::processKeyFrame(FrameBasePtr _frame) { // Search for any stored capture within time tolerance of frame - auto capture = buffer_capture_.select(_frame->getTimeStamp(), _time_tolerance); + auto capture = buffer_capture_.select(_frame->getTimeStamp(), getTimeTolerance()); if (capture) { processMatch(_frame, capture); @@ -65,10 +65,10 @@ void ProcessorCompass::processKeyFrame(FrameBasePtr _frame, const double& _time_ buffer_capture_.removeUpTo(_frame->getTimeStamp() - 10); } // Otherwise: If frame is more recent than any capture in buffer -> store frame to be processed later in processCapture - else if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), _time_tolerance) == nullptr) + else if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), getTimeTolerance()) == nullptr) { // store frame - buffer_pack_kf_.add(_frame, _time_tolerance); + buffer_frame_.emplace(_frame->getTimeStamp(), _frame); } // Otherwise: There are captures more recent than the frame but none that match with it -> discard frame } diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp index b2661cbe68b02d90813ace9ecb2094372ba81d29..5ca6bc4220ebe000537815630aa40f100b48e367 100644 --- a/test/gtest_factor_imu.cpp +++ b/test/gtest_factor_imu.cpp @@ -110,7 +110,7 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test expected_final_state = x_origin; //null bias + static //set origin of the problem - KF0 = problem->setPrior(x_origin, P_origin, t, 0.005); + KF0 = problem->setPrior(x_origin, P_origin, t); // KF0 = processor_imu->setOrigin(x_origin, t); //===================================================== END{INITIALIZATION} @@ -1241,7 +1241,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test Eigen::Quaterniond current_quatState(Eigen::Quaterniond::Identity()); //set origin of the problem - origin_KF = problem->setPrior(x_origin, P_origin, t, 0.005); + origin_KF = problem->setPrior(x_origin, P_origin, t); // origin_KF = processor_imu->setOrigin(x_origin, t); // processor_odom3d->setOrigin(origin_KF); diff --git a/test/gtest_feature_imu.cpp b/test/gtest_feature_imu.cpp index b3941de78dcecb081472103f210cadbc8b9f2712..d8e8331ce02d07657fe266cac33fc7dea391409e 100644 --- a/test/gtest_feature_imu.cpp +++ b/test/gtest_feature_imu.cpp @@ -78,7 +78,7 @@ class FeatureImu_test : public testing::Test // Set the origin VectorComposite x0("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); VectorComposite s0("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - origin_frame = problem->setPriorFactor(x0, s0, t, 0.01); + origin_frame = problem->setPriorFactor(x0, s0, t); processor_motion_ptr_->setOrigin(origin_frame); // Emplace one capture to store the Imu data arriving from (sensor / callback / file / etc.) diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp index 11c5f9c6c2d110297b820ef619711aca47503c94..4b159f7869167f24bde9ec8600ccc25c48ba01c8 100644 --- a/test/gtest_imu.cpp +++ b/test/gtest_imu.cpp @@ -349,7 +349,7 @@ class Process_Factor_Imu : public testing::Test // wolf objects WOLF_INFO("x0c: ", x0c); WOLF_INFO("s0c: ", s0c); - KF_0 = problem->setPriorFactor(x0c, s0c, t0, dt/2); + KF_0 = problem->setPriorFactor(x0c, s0c, t0); processor_imu->setOrigin(KF_0); WOLF_INFO("prior set"); @@ -508,7 +508,7 @@ class Process_Factor_Imu : public testing::Test FrameBasePtr KF = problem->emplaceFrame(t, x1_exact); // ===================================== Imu CALLBACK - problem->keyFrameCallback(KF, nullptr, dt/2); + problem->keyFrameCallback(KF, nullptr); // Process Imu for the callback to take effect data = Vector6d::Zero(); diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp index f2c3338b4f2a92b089aa1162859b0dfa6974d871..41ed32418ea52f576656541ae673c0b2440a57c1 100644 --- a/test/gtest_imu2d_static_init.cpp +++ b/test/gtest_imu2d_static_init.cpp @@ -91,8 +91,8 @@ class ProcessorImu2dStaticInitTest : public testing::Test // Set the origin VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()}); VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()}); - //KF0_ = problem_ptr_->setPriorFix(x_origin, t0, dt/2); - KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0, 0.01); + //KF0_ = problem_ptr_->setPriorFix(x_origin, t0); + KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0); } diff --git a/test/gtest_imu_mocap_fusion.cpp b/test/gtest_imu_mocap_fusion.cpp index 449313c20332600b786ec192202934a8144afe36..6c42451dcb81020ecc6b4d28f4637ebe131ecba4 100644 --- a/test/gtest_imu_mocap_fusion.cpp +++ b/test/gtest_imu_mocap_fusion.cpp @@ -113,7 +113,7 @@ class ImuMocapFusion_Test : public testing::Test solver_->getSolverOptions().max_num_iterations = 500; // initial guess VectorComposite x_origin("POV", {w_p_wb, w_q_b.coeffs(), w_v_wb}); - FrameBasePtr KF1 = problem_->setPriorInitialGuess(x_origin, 0, 0.0005); // if mocap used + FrameBasePtr KF1 = problem_->setPriorInitialGuess(x_origin, 0); // if mocap used // pose sensor and proc (to get extrinsics in the prb) auto intr_sp = std::make_shared<ParamsSensorPose>(); @@ -300,4 +300,4 @@ int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp index 7b47cd1d35e71814a9e6c96b82bdf27b0c435f33..69527fa37a6bbd984243ef49c04d870212974bc1 100644 --- a/test/gtest_imu_static_init.cpp +++ b/test/gtest_imu_static_init.cpp @@ -97,12 +97,12 @@ class ProcessorImuStaticInitTest : public testing::Test // Set the origin VectorComposite x0c("POV", {Vector3d::Zero(), (Vector4d() << 0,0,0,1).finished(), Vector3d::Zero()}); WOLF_INFO("x0c is: \n", x0c); - //KF0_ = problem_ptr_->setPriorFix(x0c, t0, dt/2); + //KF0_ = problem_ptr_->setPriorFix(x0c, t0); Vector4d q_init; q_init << 0,0,0,1; VectorComposite x_origin("POV", {Vector3d::Zero(), q_init, Vector3d::Zero()}); VectorComposite std_origin("POV", {0.001*Vector3d::Ones(), 0.001*Vector3d::Ones(), 0.001*Vector3d::Ones()}); - KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0, 0.01); + KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0); } diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu.cpp index 408a93e5871676b1bbd13d887205186069027f3a..1fe2863a6afb2488d7f9ad8f2d5b54fa8cec7b60 100644 --- a/test/gtest_processor_imu.cpp +++ b/test/gtest_processor_imu.cpp @@ -165,7 +165,7 @@ TEST(ProcessorImu, voteForKeyFrame) TimeStamp t(0); VectorComposite x0("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); VectorComposite s0("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0, s0, t, 0.01); + problem->setPriorFactor(x0, s0, t); //data variable and covariance matrix // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions @@ -233,7 +233,7 @@ TEST_F(ProcessorImut, acc_x) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -264,7 +264,7 @@ TEST_F(ProcessorImut, acc_y) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -306,7 +306,7 @@ TEST_F(ProcessorImut, acc_z) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -348,7 +348,7 @@ TEST_F(ProcessorImut, check_Covariance) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -374,7 +374,7 @@ TEST_F(ProcessorImut, gyro_x) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -431,7 +431,7 @@ TEST_F(ProcessorImut, gyro_x_biasedAbx) // init things x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -489,7 +489,7 @@ TEST_F(ProcessorImut, gyro_xy_biasedAbxy) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -545,7 +545,7 @@ TEST_F(ProcessorImut, gyro_z) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -588,7 +588,7 @@ TEST_F(ProcessorImut, gyro_xyz) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -680,7 +680,7 @@ TEST_F(ProcessorImut, gyro_z_ConstVelocity) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -724,7 +724,7 @@ TEST_F(ProcessorImut, gyro_x_ConstVelocity) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -773,7 +773,7 @@ TEST_F(ProcessorImut, gyro_xy_ConstVelocity) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -822,7 +822,7 @@ TEST_F(ProcessorImut, gyro_y_ConstVelocity) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -870,7 +870,7 @@ TEST_F(ProcessorImut, gyro_xyz_ConstVelocity) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -964,7 +964,7 @@ TEST_F(ProcessorImut, gyro_x_acc_x) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -1018,7 +1018,7 @@ TEST_F(ProcessorImut, gyro_y_acc_y) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -1072,7 +1072,7 @@ TEST_F(ProcessorImut, gyro_z_acc_z) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); diff --git a/test/gtest_processor_imu2d.cpp b/test/gtest_processor_imu2d.cpp index 12d91d550645c60af2bb7707a61acc02ef89db1a..b13d5d27a3dca5cc255c913de06f34ca6574957e 100644 --- a/test/gtest_processor_imu2d.cpp +++ b/test/gtest_processor_imu2d.cpp @@ -134,7 +134,7 @@ TEST_F(ProcessorImu2dTest, Prior) x0c['P'] = Vector2d(1,2); x0c['O'] = Vector1d(0); x0c['V'] = Vector2d(4,5); - auto KF0 = problem->setPriorFix(x0c, t0, dt/2); + auto KF0 = problem->setPriorFix(x0c, t0); processor_motion->setOrigin(KF0); //WOLF_DEBUG("x0 =", x0c); //WOLF_DEBUG("KF0 state =", problem->getTrajectory()->getFrameMap().at(t)->getState("POV")); @@ -148,7 +148,7 @@ TEST_F(ProcessorImu2dTest, MRUA) x0c['P'] = Vector2d(1,2); x0c['O'] = Vector1d(0); x0c['V'] = Vector2d(4,5); - auto KF0 = problem->setPriorFix(x0c, t0, dt/2); + auto KF0 = problem->setPriorFix(x0c, t0); processor_motion->setOrigin(KF0); //WOLF_DEBUG("Current State: ", problem->getState()); @@ -175,7 +175,7 @@ TEST_F(ProcessorImu2dTest, parabola) x0c['V'] = Vector2d(v0, 0); data_cov *= 1e-3; - auto KF0 = problem->setPriorFix(x0c, t0, dt/2); + auto KF0 = problem->setPriorFix(x0c, t0); processor_motion->setOrigin(KF0); for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){ @@ -205,7 +205,7 @@ TEST_F(ProcessorImu2dTest, parabola_deluxe) x0c['V'] = v0; data_cov *= 1e-3; - auto KF0 = problem->setPriorFix(x0c, t0, dt/2); + auto KF0 = problem->setPriorFix(x0c, t0); processor_motion->setOrigin(KF0); for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){ @@ -237,7 +237,7 @@ TEST_F(ProcessorImu2dTest, Circular_move) data_cov *= 1e-3; //dt = 0.0001; - auto KF0 = problem->setPriorFix(x0c, t0, dt/2); + auto KF0 = problem->setPriorFix(x0c, t0); processor_motion->setOrigin(KF0); WOLF_DEBUG("Current State: ", problem->getState()); diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp index 64d4a8f0b3b4672a341cf4d8bf4e09ccf5754fbd..fc7516925ecdbc3d7175f0522cc5a246310291e1 100644 --- a/test/gtest_processor_motion_intrinsics_update.cpp +++ b/test/gtest_processor_motion_intrinsics_update.cpp @@ -98,7 +98,7 @@ class ProcessorImuTest : public testing::Test params_->time_tolerance = 0.0025; processor_ = std::static_pointer_cast<ProcessorImu>(problem_->installProcessor("ProcessorImu", "processor imu", sensor_, params_)); - KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0, 0.01); + KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0); Vector6d bias; bias << 0.42,0,0, 0,0,0; // Vector6d bias; bias << 0.0,0,0, 0.42,0,0;