diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 254bdb6a95115be348c75c9086cbe444e842c42e..e1c976188cbd5898a37c1c0c35a2ce1f6f0695da 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -173,8 +173,8 @@ ADD_EXECUTABLE(test_processor_imu_jacobians test_processor_imu_jacobians.cpp) TARGET_LINK_LIBRARIES(test_processor_imu_jacobians ${PROJECT_NAME}) # IMU - constraintAHP -ADD_EXECUTABLE(test_imu_constraintAHP test_imu_constraintAHP.cpp) -TARGET_LINK_LIBRARIES(test_imu_constraintAHP ${PROJECT_NAME}) +ADD_EXECUTABLE(test_imu_constrained0 test_imu_constrained0.cpp) +TARGET_LINK_LIBRARIES(test_imu_constrained0 ${PROJECT_NAME}) # IF (laser_scan_utils_FOUND) # ADD_EXECUTABLE(test_capture_laser_2D test_capture_laser_2D.cpp) diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp index 75ea63c475543ae6efa60b234a5100296c63d0d4..77e10258c759f205ec8ffe351e2562f077b5ea03 100644 --- a/src/examples/test_imu_constrained0.cpp +++ b/src/examples/test_imu_constrained0.cpp @@ -3,6 +3,7 @@ #include "problem.h" #include "sensor_imu.h" #include "capture_imu.h" +#include "constraint_odom_3D.h" #include "state_block.h" #include "state_quaternion.h" #include "processor_imu.h" @@ -54,7 +55,7 @@ int main(int argc, char** argv) } // Wolf problem - ProblemPtr wolf_problem_ptr_ = Problem::create(FRM_PVQBB_3D); + ProblemPtr wolf_problem_ptr_ = Problem::create(FRM_PQVBB_3D); Eigen::VectorXs IMU_extrinsics(7); IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, shared_ptr<IntrinsicsBase>()); @@ -74,19 +75,21 @@ int main(int argc, char** argv) //create a keyframe at origin TimeStamp ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_; - Eigen::VectorXs state_vec = x0; - FrameBasePtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); - wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame); + Eigen::VectorXs origin_state = x0; + FrameBasePtr origin_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, origin_state); + wolf_problem_ptr_->getTrajectoryPtr()->addFrame(origin_frame); // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.) - CaptureIMU::Ptr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_) ); + CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_) ); // main loop using namespace std; clock_t begin = clock(); const int keyframe_spacing = 10; int last_keyframe_dt = 0; - + Eigen::VectorXs state_vec; + FrameBasePtr last_frame; + while(!data_file.eof()){ if(last_keyframe_dt >= keyframe_spacing){ ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_; @@ -119,6 +122,11 @@ int main(int argc, char** argv) last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame); + //create a feature + FeatureBasePtr last_feature = std::make_shared<FeatureBase>("ODOM_3D", origin_state.head(7),Eigen::Matrix7s::Identity()); + + //create an ODOM constraint between first and last keyframes + ConstraintOdom3DPtr constraint_ptr = std::make_shared<ConstraintOdom3D>(last_feature, last_frame); clock_t end = clock(); double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;