diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 99c97e854a19d24cc8b6899f400ffc1616bb170d..918f0385de6b51a1005b741b5ec315e27646bd86 100644
--- a/src/CMakeLists.txt
+++ b/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
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index f790a6172815cdd83f2789ca572f096a340cc9ff..a66b289b7a4777401c608818447feaf404d9e2d0 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/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());
}
}
}
diff --git a/src/problem.cpp b/src/problem.cpp
index a40d1b3e05c9113c39b9a26c63c172cbb95b5445..c9eeb21c041a8643f97808a45a316a88f4661873 100644
--- a/src/problem.cpp
+++ b/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);
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index e17c407c9f6a127d2f2ad73dd8d3e7751a8f918a..0cbdd2863930306a61213126f905714f60d0a6f7 100644
--- a/src/processor_motion.cpp
+++ b/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)
{
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 9b8bd4e6fbd02a545b66fc4868cdc85ed995465f..cd36d2881ca75aaeb8116c84b9678feac37e8663 100644
--- a/src/sensor_base.h
+++ b/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);
}
diff --git a/src/wolf.h b/src/wolf.h
index 65df8456b65922f0a8a66026d02d5db70484ddb0..dc63786c3e1f45347d305c718bfe99144803760f 100644
--- a/src/wolf.h
+++ b/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