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

frame creation -> proposed fix (to be checked)

parent e9d1a6c2
No related branches found
No related tags found
No related merge requests found
...@@ -131,12 +131,16 @@ void FrameBase::setState(const Eigen::VectorXs& _st) ...@@ -131,12 +131,16 @@ void FrameBase::setState(const Eigen::VectorXs& _st)
size += (state_block_vec_[i]==nullptr ? 0 : state_block_vec_[i]->getSize()); size += (state_block_vec_[i]==nullptr ? 0 : state_block_vec_[i]->getSize());
} }
assert(_st.size() == size && "In FrameBase::setState wrong state size"); //State Vector size must be lower or equal to frame state size :
// example : PQVBB -> if we initialize only position and orientation due to use of processorOdom3D
assert(_st.size() <= size && "In FrameBase::setState wrong state size");
unsigned int index = 0; unsigned int index = 0;
const unsigned int _st_size = _st.size();
//initialize the FrameBase StateBlocks while StateBlocks list's end not reached and input state_size can go further
for (StateBlockPtr sb : state_block_vec_) for (StateBlockPtr sb : state_block_vec_)
if (sb) if (sb && (index < _st_size))
{ {
sb->setVector(_st.segment(index, sb->getSize())); sb->setVector(_st.segment(index, sb->getSize()));
index += sb->getSize(); index += sb->getSize();
......
...@@ -563,7 +563,13 @@ void Problem::setOrigin(const Eigen::VectorXs& _origin_state, const Eigen::Matri ...@@ -563,7 +563,13 @@ void Problem::setOrigin(const Eigen::VectorXs& _origin_state, const Eigen::Matri
// CaptureFixPtr init_capture = std::make_shared<CaptureFix>(_ts, nullptr, pose, pose_cov); // CaptureFixPtr init_capture = std::make_shared<CaptureFix>(_ts, nullptr, pose, pose_cov);
// create origin capture with the given state as data // create origin capture with the given state as data
CaptureFixPtr init_capture = std::make_shared<CaptureFix>(_ts, nullptr, _origin_state, _origin_state_cov); // Capture fix only takes 3D position and Quaternion orientation
CaptureFixPtr init_capture;
if ((trajectory_ptr_->getFrameStructure() == FRM_PQVBB_3D) || (trajectory_ptr_->getFrameStructure() == FRM_POV_3D) )
init_capture = std::make_shared<CaptureFix>(_ts, nullptr, _origin_state.head(7), _origin_state_cov);
else
init_capture = std::make_shared<CaptureFix>(_ts, nullptr, _origin_state, _origin_state_cov);
origin_frame_ptr->addCapture(init_capture); origin_frame_ptr->addCapture(init_capture);
// create feature and constraint // create feature and constraint
......
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