Merge branch 'stuff' into 'master'

Miscellaneous

See merge request !126

src/CMakeLists.txt

@@ -157,6 +157,7 @@ SET(HDRS_BASE
     constraint_analytic.h
     constraint_base.h
     constraint_sparse.h
+    factory.h
     feature_base.h
     feature_match.h
     frame_base.h

src/ceres_wrapper/ceres_manager.cpp

@@ -105,8 +105,8 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
             {
                 for  (unsigned int j = i; j < all_state_blocks.size(); j++)
                 {
-                    state_block_pairs.push_back(std::make_pair(all_state_blocks[i],all_state_blocks[j]));
-                    double_pairs.push_back(std::make_pair(all_state_blocks[i]->getPtr(),all_state_blocks[j]->getPtr()));
+                    state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
+                    double_pairs.emplace_back(all_state_blocks[i]->getPtr(),all_state_blocks[j]->getPtr());
                 }
             }
             break;
@@ -128,8 +128,8 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
                             {
                                 if (sb)
                                 {
-                                    state_block_pairs.push_back(std::make_pair(sb, sb2));
-                                    double_pairs.push_back(std::make_pair(sb->getPtr(), sb2->getPtr()));
+                                    state_block_pairs.emplace_back(sb, sb2);
+                                    double_pairs.emplace_back(sb->getPtr(), sb2->getPtr());
                                     if (sb == sb2) break;
                                 }
                             }
@@ -151,8 +151,8 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
 //                    all_state_blocks.push_back(sb);
                     for(auto sb2 : l_ptr->getUsedStateBlockVec())
                     {
-                        state_block_pairs.push_back(std::make_pair(sb, sb2));
-                        double_pairs.push_back(std::make_pair(sb->getPtr(), sb2->getPtr()));
+                        state_block_pairs.emplace_back(sb, sb2);
+                        double_pairs.emplace_back(sb->getPtr(), sb2->getPtr());
                         if (sb == sb2) break;
                     }
                 }
@@ -163,13 +163,13 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
 //            // loop all marginals (PO marginals)
 //            for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
 //            {
-//                state_block_pairs.push_back(std::make_pair(all_state_blocks[2*i],all_state_blocks[2*i]));
-//                state_block_pairs.push_back(std::make_pair(all_state_blocks[2*i],all_state_blocks[2*i+1]));
-//                state_block_pairs.push_back(std::make_pair(all_state_blocks[2*i+1],all_state_blocks[2*i+1]));
+//                state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i]);
+//                state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i+1]);
+//                state_block_pairs.emplace_back(all_state_blocks[2*i+1],all_state_blocks[2*i+1]);
 //
-//                double_pairs.push_back(std::make_pair(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i]->getPtr()));
-//                double_pairs.push_back(std::make_pair(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i+1]->getPtr()));
-//                double_pairs.push_back(std::make_pair(all_state_blocks[2*i+1]->getPtr(),all_state_blocks[2*i+1]->getPtr()));
+//                double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i]->getPtr());
+//                double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i+1]->getPtr());
+//                double_pairs.emplace_back(all_state_blocks[2*i+1]->getPtr(),all_state_blocks[2*i+1]->getPtr());
 //            }
             break;
         }
@@ -178,13 +178,13 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
             //robot-robot
             auto last_key_frame = wolf_problem_->getLastKeyFramePtr();
 
-            state_block_pairs.push_back(std::make_pair(last_key_frame->getPPtr(), last_key_frame->getPPtr()));
-            state_block_pairs.push_back(std::make_pair(last_key_frame->getPPtr(), last_key_frame->getOPtr()));
-            state_block_pairs.push_back(std::make_pair(last_key_frame->getOPtr(), last_key_frame->getOPtr()));
+            state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getPPtr());
+            state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getOPtr());
+            state_block_pairs.emplace_back(last_key_frame->getOPtr(), last_key_frame->getOPtr());
 
-            double_pairs.push_back(std::make_pair(last_key_frame->getPPtr()->getPtr(), last_key_frame->getPPtr()->getPtr()));
-            double_pairs.push_back(std::make_pair(last_key_frame->getPPtr()->getPtr(), last_key_frame->getOPtr()->getPtr()));
-            double_pairs.push_back(std::make_pair(last_key_frame->getOPtr()->getPtr(), last_key_frame->getOPtr()->getPtr()));
+            double_pairs.emplace_back(last_key_frame->getPPtr()->getPtr(), last_key_frame->getPPtr()->getPtr());
+            double_pairs.emplace_back(last_key_frame->getPPtr()->getPtr(), last_key_frame->getOPtr()->getPtr());
+            double_pairs.emplace_back(last_key_frame->getOPtr()->getPtr(), last_key_frame->getOPtr()->getPtr());
 
             // landmarks
             std::vector<StateBlockPtr> landmark_state_blocks;
@@ -196,16 +196,16 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
                 for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
                 {
                     // robot - landmark
-                    state_block_pairs.push_back(std::make_pair(last_key_frame->getPPtr(), *state_it));
-                    state_block_pairs.push_back(std::make_pair(last_key_frame->getOPtr(), *state_it));
-                    double_pairs.push_back(std::make_pair(last_key_frame->getPPtr()->getPtr(), (*state_it)->getPtr()));
-                    double_pairs.push_back(std::make_pair(last_key_frame->getOPtr()->getPtr(), (*state_it)->getPtr()));
+                    state_block_pairs.emplace_back(last_key_frame->getPPtr(), *state_it);
+                    state_block_pairs.emplace_back(last_key_frame->getOPtr(), *state_it);
+                    double_pairs.emplace_back(last_key_frame->getPPtr()->getPtr(), (*state_it)->getPtr());
+                    double_pairs.emplace_back(last_key_frame->getOPtr()->getPtr(), (*state_it)->getPtr());
 
                     // landmark marginal
                     for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
                     {
-                        state_block_pairs.push_back(std::make_pair(*state_it, *next_state_it));
-                        double_pairs.push_back(std::make_pair((*state_it)->getPtr(), (*next_state_it)->getPtr()));
+                        state_block_pairs.emplace_back(*state_it, *next_state_it);
+                        double_pairs.emplace_back((*state_it)->getPtr(), (*next_state_it)->getPtr());
                     }
                 }
             }

src/problem.cpp

@@ -484,12 +484,12 @@ bool Problem::getFrameCovariance(FrameBasePtr _frame_ptr, Eigen::MatrixXs& _cova
     bool success(true);
     int i = 0, j = 0;
 
-    for (auto sb_i : _frame_ptr->getStateBlockVec())
+    for (const auto& sb_i : _frame_ptr->getStateBlockVec())
     {
         if (sb_i)
         {
             j = 0;
-            for (auto sb_j : _frame_ptr->getStateBlockVec())
+            for (const auto& sb_j : _frame_ptr->getStateBlockVec())
             {
                 if (sb_j)
                 {
@@ -506,7 +506,7 @@ bool Problem::getFrameCovariance(FrameBasePtr _frame_ptr, Eigen::MatrixXs& _cova
 Eigen::MatrixXs Problem::getFrameCovariance(FrameBasePtr _frame_ptr)
 {
     Size sz = 0;
-    for (auto sb : _frame_ptr->getStateBlockVec())
+    for (const auto& sb : _frame_ptr->getStateBlockVec())
         if (sb)
             sz += sb->getSize();
     Eigen::MatrixXs covariance(sz, sz);

src/processor_motion.cpp

@@ -35,7 +35,11 @@ ProcessorMotion::~ProcessorMotion()
 
 void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
 {
-
+  if (_incoming_ptr == nullptr)
+  {
+    WOLF_ERROR("Process got a nullptr !");
+    return;
+  }
 
     if (status_ == IDLE)
     {

src/sensor_base.h

@@ -256,9 +256,11 @@ inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr)
 
 inline bool SensorBase::process(const CaptureBasePtr capture_ptr)
 {
+  if (capture_ptr == nullptr) return false;
+
   capture_ptr->setSensorPtr(shared_from_this());
 
-  for (const auto processor : processor_list_)
+  for (const auto& processor : processor_list_)
   {
     processor->process(capture_ptr);
   }

src/wolf.h

@@ -89,6 +89,7 @@ typedef Matrix<wolf::Scalar, 1, 1> Vector1s;                ///< 1-vector of rea
 typedef Matrix<wolf::Scalar, 2, 1> Vector2s;                ///< 2-vector of real Scalar type
 typedef Matrix<wolf::Scalar, 3, 1> Vector3s;                ///< 3-vector of real Scalar type
 typedef Matrix<wolf::Scalar, 4, 1> Vector4s;                ///< 4-vector of real Scalar type
+typedef Matrix<wolf::Scalar, 5, 1> Vector5s;                ///< 5-vector of real Scalar type
 typedef Matrix<wolf::Scalar, 6, 1> Vector6s;                ///< 6-vector of real Scalar type
 typedef Matrix<wolf::Scalar, 7, 1> Vector7s;                ///< 7-vector of real Scalar type
 typedef Matrix<wolf::Scalar, Dynamic, 1> VectorXs;          ///< variable size vector of real Scalar type
@@ -102,6 +103,8 @@ typedef Matrix<wolf::Scalar, 1, Dynamic> RowVectorXs;       ///< variable size r
 typedef Quaternion<wolf::Scalar> Quaternions;               ///< Quaternion of real Scalar type
 typedef AngleAxis<wolf::Scalar> AngleAxiss;                 ///< Angle-Axis of real Scalar type
 typedef Rotation2D<wolf::Scalar> Rotation2Ds;               ///< Rotation2D of real Scalar type
+
+typedef Transform<wolf::Scalar,3,Affine> Affine3ds;         ///< Affine3d of real Scalar type
 }
 
 namespace wolf {
@@ -326,6 +329,38 @@ WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
 inline const Eigen::Vector3s gravity(void) {
     return Eigen::Vector3s(0,0,-9.806);
 }
+
+template <typename T, int N>
+bool isSymmetric(const Eigen::Matrix<T, N, N>& M,
+                 const T eps = wolf::Constants::EPS)
+{
+  return M.isApprox(M.transpose(), eps);
+}
+
+template <typename T, int N>
+bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N>& M)
+{
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N> > eigensolver(M);
+
+  if (eigensolver.info() == Eigen::Success)
+  {
+    // All eigenvalues must be >= 0:
+    return (eigensolver.eigenvalues().array() >= T(0)).all();
+  }
+
+  return false;
+}
+
+template <typename T, int N>
+bool isCovariance(const Eigen::Matrix<T, N, N>& M)
+{
+  return isSymmetric(M) && isPositiveSemiDefinite(M);
+}
+
+#define WOLF_ASSERT_COVARIANCE_MATRIX(x) \
+  assert(x.determinant() > 0 && "Not positive definite measurement covariance"); \
+  assert(isCovariance(x) && "Not a covariance");
+
 //===================================================
 
 } // namespace wolf