working gtest_param_prior

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));

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)
 {