Skip to content
Snippets Groups Projects
Commit 3df36dfc authored by Dinesh Atchuthan's avatar Dinesh Atchuthan
Browse files

[WIP] gtest_constraint_odom - sensor installation

parent ea76c6f2
No related branches found
No related tags found
No related merge requests found
...@@ -23,23 +23,59 @@ TEST(ConstraintOdom3D, constructors) ...@@ -23,23 +23,59 @@ TEST(ConstraintOdom3D, constructors)
using std::make_shared; using std::make_shared;
using std::static_pointer_cast; using std::static_pointer_cast;
std::cout << "start" << std::endl;
// Wolf problem // Wolf problem
wolf::ProblemPtr wolf_problem_ptr_ = Problem::create(FRM_PQVBB_3D); wolf::ProblemPtr wolf_problem_ptr_ = Problem::create(FRM_PO_3D);
Eigen::VectorXs IMU_extrinsics(7); 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; std::cout << "debug" << std::endl;
Eigen::VectorXs state_vec; ProcessorOdom3D prc;
Eigen::VectorXs delta_preint; Motion ref(0,7,6), final(0,7,6);
Eigen::Vector6s data_; Eigen::VectorXs origin_state(7);
TimeStamp t_o, ts;
state_vec.resize(16); // set ref
ts.set(0); 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); // set final
x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; // Try some non-zero biases 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 //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) int main(int argc, char **argv)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment