diff --git a/src/test/gtest_constraint_odom_3D.cpp b/src/test/gtest_constraint_odom_3D.cpp index 49e140de7f95b7ca87fae6e28e0f6706c11d2a03..344b0fb5be610d7a00c1e8d24b440547ef761dc3 100644 --- a/src/test/gtest_constraint_odom_3D.cpp +++ b/src/test/gtest_constraint_odom_3D.cpp @@ -23,23 +23,59 @@ TEST(ConstraintOdom3D, constructors) using std::make_shared; using std::static_pointer_cast; + std::cout << "start" << std::endl; // Wolf problem - wolf::ProblemPtr wolf_problem_ptr_ = Problem::create(FRM_PQVBB_3D); - Eigen::VectorXs IMU_extrinsics(7); + wolf::ProblemPtr wolf_problem_ptr_ = Problem::create(FRM_PO_3D); + std::cout << "started" << std::endl; + //just to make it work + Eigen::VectorXs extrinsics(7); + extrinsics << 0,0,0, 0,0,0,1; // pose in the robot + std::cout << "sensor installing" << std::endl; + SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "Main ODOM_3D", extrinsics, shared_ptr<IntrinsicsOdom3D>()); + std::cout << "sensor installed" << std::endl; + wolf_problem_ptr_->installProcessor("ODOM 3D", "ODOM_3D integrator", "Main ODOM_3D", ""); - wolf::TimeStamp ts; - Eigen::VectorXs state_vec; - Eigen::VectorXs delta_preint; - Eigen::Vector6s data_; + std::cout << "debug" << std::endl; + ProcessorOdom3D prc; + Motion ref(0,7,6), final(0,7,6); + Eigen::VectorXs origin_state(7); + TimeStamp t_o, ts; - state_vec.resize(16); - ts.set(0); + // set ref + ref.ts_ = 0; + ref.delta_ << 0,0,0, 0,0,0,1; + ref.delta_integr_ << 0,0,0, 0,0,0,1; - Eigen::VectorXs x0(16); - x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; // Try some non-zero biases + // set final + final.ts_ = 10; + final.delta_ << 1,5,0.5, 0,0,0,1; + final.delta_integr_ << 0,0,0, 0,0,0,1; + //set origin state + origin_state << 0,0,0, 0,0,0,1; + t_o.set(0); + + std::cout << "debug" << std::endl; + prc.setOrigin(origin_state, t_o); + std::cout << "debug" << std::endl; + prc.deltaPlusDelta(ref.delta_integr_, final.delta_, (final.ts_ - ref.ts_), final.delta_integr_); + + //create a keyframe at origin + StateBlockPtr o_p = std::make_shared<StateBlock>(origin_state.head<3>()); + StateBlockPtr o_q = std::make_shared<StateBlock>(origin_state.tail<3>()); + wolf::FrameBasePtr origin_frame = std::make_shared<FrameBase>(t_o, o_p, o_q); + wolf_problem_ptr_->getTrajectoryPtr()->addFrame(origin_frame); + //create a keyframe at final state + ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_; + Eigen::VectorXs final_state; + final_state = prc.getCurrentState().head(7); + StateBlockPtr f_p = std::make_shared<StateBlock>(final_state.head<3>()); + StateBlockPtr f_q = std::make_shared<StateBlock>(final_state.tail<3>()); + wolf::FrameBasePtr final_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, f_p, f_q); + wolf_problem_ptr_->getTrajectoryPtr()->addFrame(final_frame); + //create a feature - FeatureBasePtr last_feature = std::make_shared<FeatureBase>("ODOM_3D", x0.head(7),Eigen::Matrix7s::Identity()); + //FeatureBasePtr last_feature = std::make_shared<FeatureBase>("ODOM_3D", x0.head(7),Eigen::Matrix7s::Identity()); } int main(int argc, char **argv)