From c1b00d8d6563e17f0f5e0d9166dcb25225a70bc8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Wed, 6 Feb 2019 16:31:36 +0100 Subject: [PATCH] working gtest_param_prior --- src/sensor_base.cpp | 13 +++++++----- src/test/gtest_param_prior.cpp | 39 ++++++++++++++++++++++------------ 2 files changed, 34 insertions(+), 18 deletions(-) diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp index 954f519ad..96f0403f6 100644 --- a/src/sensor_base.cpp +++ b/src/sensor_base.cpp @@ -148,24 +148,27 @@ void SensorBase::unfixIntrinsics() void SensorBase::addParameterPrior(const StateBlockPtr& _sb, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size) { + bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr); + assert(std::find(state_block_vec_.begin(),state_block_vec_.end(),_sb) != state_block_vec_.end() && "adding prior to unknown state block"); - assert(_x.size() == _cov.rows() && _x.size() == _cov.cols() && "covariance and prior dimension should be the same"); + assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) || + (is_quaternion && _x.size() == 4 &&_cov.rows() == 3 && _cov.cols() == 3)) && "bad prior/covariance dimensions"); assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize())); assert(_size == -1 || _size == _x.size()); - assert(!(_size != -1 && _sb->hasLocalParametrization()) && "prior for a segment of the state only available withour local parameterization"); + assert(!(_size != -1 && is_quaternion) && "prior of a segment of state not available for quaternion"); // set StateBlock state if (_size == -1) _sb->setState(_x); else { - auto new_x = _sb->getState(); + Eigen::VectorXs new_x = _sb->getState(); new_x.segment(_start_idx,_size) = _x; _sb->setState(new_x); } // remove previous prior (if any) - if (params_prior_map_.find(_sb) == params_prior_map_.end()) + if (params_prior_map_.find(_sb) != params_prior_map_.end()) params_prior_map_[_sb]->remove(); // create feature @@ -175,7 +178,7 @@ void SensorBase::addParameterPrior(const StateBlockPtr& _sb, const Eigen::Vector ftr_prior->setProblem(getProblem()); // create & add constraint absolute - if (std::dynamic_pointer_cast<StateQuaternion>(_sb)) + if (is_quaternion) ftr_prior->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(_sb)); else ftr_prior->addConstraint(std::make_shared<ConstraintBlockAbsolute>(_sb, _start_idx, _size)); diff --git a/src/test/gtest_param_prior.cpp b/src/test/gtest_param_prior.cpp index 08d47cec1..830660489 100644 --- a/src/test/gtest_param_prior.cpp +++ b/src/test/gtest_param_prior.cpp @@ -18,11 +18,11 @@ using namespace wolf; ProblemPtr problem_ptr = Problem::create("PO 3D"); CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr); -Eigen::Vector3s initial_extrinsics((Eigen::Vector3s() << 1, 2, 3, 1, 0, 0, 0).finished()); -Eigen::Vector3s prior_extrinsics((Eigen::Vector3s() << 10, 20, 30, 0, 0, 0, 1).finished()); +Eigen::Vector7s initial_extrinsics((Eigen::Vector7s() << 1, 2, 3, 1, 0, 0, 0).finished()); +Eigen::Vector7s prior_extrinsics((Eigen::Vector7s() << 10, 20, 30, 0, 0, 0, 1).finished()); +Eigen::Vector7s prior2_extrinsics((Eigen::Vector7s() << 100, 200, 300, 0, 0, 0, 1).finished()); SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("ODOM 3D", "odometer", initial_extrinsics, std::make_shared<IntrinsicsOdom3D>())); -SensorOdom3DPtr odom_sensor2_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("ODOM 3D", "odometer2", initial_extrinsics, std::make_shared<IntrinsicsOdom3D>())); TEST(ParameterPrior, initial_extrinsics) { @@ -35,34 +35,47 @@ TEST(ParameterPrior, initial_extrinsics) TEST(ParameterPrior, prior_p) { - odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getPPtr(), prior_extrinsics,Eigen::Matrix1s::Identity()); + odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getPPtr(), prior_extrinsics.head(3),Eigen::Matrix3s::Identity()); - // solve for frm1 std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState().tail(1),prior_extrinsics.segment(1,1),1e-6); + ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState(),prior_extrinsics.head(3),1e-6); } TEST(ParameterPrior, prior_o) { - odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getOPtr(), prior_extrinsics.tail(1),Eigen::Matrix1s::Identity()); + odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getOPtr(), prior_extrinsics.tail(4),Eigen::Matrix3s::Identity()); - // solve for frm1 std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getOPtr()->getState(),prior_extrinsics.tail(1),1e-6); + ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getOPtr()->getState(),prior_extrinsics.tail(4),1e-6); } -TEST(ParameterPrior, prior_p_tail) +TEST(ParameterPrior, prior_p_overwrite) { - odom_sensor2_ptr_->addParameterPrior(odom_sensor2_ptr_->getPPtr(), prior_extrinsics.segment(1,2),Eigen::Matrix1s::Identity(),1,2); + odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getPPtr(), prior2_extrinsics.head(3),Eigen::Matrix3s::Identity()); - // solve for frm1 std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_MATRIX_APPROX(odom_sensor2_ptr_->getPPtr()->getState().tail(2),prior_extrinsics.segment(1,2),1e-6); + ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState(),prior2_extrinsics.head(3),1e-6); } +TEST(ParameterPrior, prior_p_segment) +{ + odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getPPtr(), prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2); + + std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF); + + ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6); +} + +TEST(ParameterPrior, prior_o_segment) +{ + // constraining segment of quaternion is not allowed + ASSERT_DEATH(odom_sensor_ptr_->addParameterPrior(odom_sensor_ptr_->getOPtr(), prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2),""); +} + + int main(int argc, char **argv) { -- GitLab