diff --git a/CMakeLists.txt b/CMakeLists.txt
index 75b2269f91251bf68673b37ba86962ee03878e1a..0910fe579821a1fc1ab7e2df324c754212584123 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -356,6 +356,7 @@ SET(SRCS_LANDMARK
src/landmark/landmark_base.cpp
)
SET(SRCS_PROCESSOR
+ src/processor/is_motion.cpp
src/processor/motion_buffer.cpp
src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 68db6b50e0abd0e2be06711df994cecd3dcc4693..b1309f336c0f8979d5cc4fa8d57aad37de2a00dc 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -221,19 +221,7 @@ int main()
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
- for (auto& sen : problem->getHardware()->getSensorList())
- for (auto& sb : sen->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto& kf : *problem->getTrajectory())
- if (kf->isKeyOrAux())
- for (auto& pair_key_sb : kf->getStateBlockMap())
- if (!pair_key_sb.second->isFixed())
- pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
- for (auto lmk : problem->getMap()->getLandmarkList())
- for (auto& pair_key_sb : lmk->getStateBlockMap())
- if (!pair_key_sb.second->isFixed())
- pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+ problem->perturb(0.5); // Perturb all state blocks that are not fixed
problem->print(1,0,1,0);
// SOLVE again
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 9b1e749cda92ff0a039816c63f2c753d144e907b..cad0474fa06cc72c6df41d8f95f20153c15fa06e 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -206,19 +206,7 @@ int main()
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
- for (auto& sen : problem->getHardware()->getSensorList())
- for (auto& sb : sen->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto& kf : *problem->getTrajectory())
- if (kf->isKeyOrAux())
- for (auto& pair_key_sb : kf->getStateBlockMap())
- if (pair_key_sb.second && !pair_key_sb.second->isFixed())
- pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
- for (auto lmk : problem->getMap()->getLandmarkList())
- for (auto& pair_key_sb : lmk->getStateBlockMap())
- if (!pair_key_sb.second->isFixed())
- pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+ problem->perturb(0.5); // Perturb all state blocks that are not fixed
problem->print(1,0,1,0);
// SOLVE again
@@ -253,9 +241,8 @@ int main()
*
* IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
*
- * L3 POINT 2d <-- c8
- * Est, x = ( 3 2 )
- * sb: Est
+ * Lmk3 LandmarkPoint2d <-- Fac9
+ * P[Est] = ( 1 2 )
*
* it means WOLF SOLVED SUCCESSFULLY (L3 is effectively at location (3,2) ) !
*
@@ -274,79 +261,79 @@ int main()
* The full message is explained below.
*
* P: wolf tree status ---------------------
- Hardware
- Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D
- sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
- PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D
- o: Cap6 - KFrm3 // origin at Capture 6, Frame 3
- l: Cap8 - Frm4 // last at Capture 8, Frame 4
- Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing
- sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
- Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type rande and bearing
- Trajectory
- KFrm1 <-- Fac4 //KeyFrame 1, constrained by Factor 4
- P[Est] = ( -4.4e-12 1.5e-09 ) //Position is estimated
- O[Est] = ( 2.6e-09 ) //Orientation is estimated
- Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior
- Ftr1 trk0 prior <-- // Position prior
- m = ( 0 0 )
- Fac1 FactorBlockAbsolute --> Abs
- Ftr2 trk0 prior <-- // Orientation prior
- m = ( 0 )
- Fac2 FactorBlockAbsolute --> Abs
- CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1.
- buffer size : 0
- Cap4 CaptureRangeBearing -> Sen2 <--
- Ftr3 trk0 FeatureRangeBearing <--
- m = ( 1 1.6 )
- Fac3 RANGE BEARING --> Lmk1
- KFrm2 <-- Fac7
- P[Est] = ( 1 5.7e-09 )
- O[Est] = ( 5.7e-09 )
- CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <-- // Capture 3 of type motion odom2d from sensor 1.
+ Hardware
+ Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D
+ sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
+ PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D
+ o: Cap6 - KFrm3 // origin at Capture 6, Frame 3
+ l: Cap8 - Frm4 // last at Capture 8, Frame 4
+ Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing
+ sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
+ Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type range and bearing
+ Trajectory
+ KFrm1 <-- Fac4 // KeyFrame 1, constrained by Factor 4
+ P[Est] = ( -4.4e-12 1.5e-09 ) // Position is estimated
+ O[Est] = ( 2.6e-09 ) // Orientation is estimated
+ Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior
+ Ftr1 trk0 prior <-- // Position prior
+ m = ( 0 0 )
+ Fac1 FactorBlockAbsolute --> Abs
+ Ftr2 trk0 prior <-- // Orientation prior
+ m = ( 0 )
+ Fac2 FactorBlockAbsolute --> Abs
+ CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1.
+ buffer size : 0
+ Cap4 CaptureRangeBearing -> Sen2 <--
+ Ftr3 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac3 RANGE BEARING --> Lmk1
+ KFrm2 <-- Fac7
+ P[Est] = ( 1 5.7e-09 )
+ O[Est] = ( 5.7e-09 )
+ CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <--// Capture 3 of type motion odom2d from sensor 1.
// Its origin is at Capture 2; Frame 1
- buffer size : 2
- delta preint : (1 0 0)
- Ftr4 trk0 ProcessorOdom2d <--
- m = ( 1 0 0 )
- Fac4 FactorOdom2d --> Frm1
- Cap7 CaptureRangeBearing -> Sen2 <--
- Ftr5 trk0 FeatureRangeBearing <--
- m = ( 1.4 2.4 )
- Fac5 RANGE BEARING --> Lmk1
- Ftr6 trk0 FeatureRangeBearing <--
- m = ( 1 1.6 )
- Fac6 RANGE BEARING --> Lmk2
- KFrm3 <--
- P[Est] = ( 2 1.2e-08 )
- O[Est] = ( 6.6e-09 )
- CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <--
- buffer size : 2
- delta preint : (1 0 0)
- Ftr7 trk0 ProcessorOdom2d <--
- m = ( 1 0 0 )
- Fac7 FactorOdom2d --> Frm2
- Cap9 CaptureRangeBearing -> Sen2 <--
- Ftr8 trk0 FeatureRangeBearing <--
- m = ( 1.4 2.4 )
- Fac8 RANGE BEARING --> Lmk2
- Ftr9 trk0 FeatureRangeBearing <--
- m = ( 1 1.6 )
- Fac9 RANGE BEARING --> Lmk3
- Frm4 <--
- P[Est] = ( 2 0 )
- O[Est] = ( 0 )
- CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <--
- buffer size : 1
- delta preint : (0 0 0)
- Map
- Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by facotrs 3 & 5
- P[Est] = ( 1 2 )
- Lmk2 LandmarkPoint2d <-- Fac6 Fac8
- P[Est] = ( 2 2 )
- Lmk3 LandmarkPoint2d <-- Fac9
- P[Est] = ( 3 2 )
- -----------------------------------------
+ buffer size : 2
+ delta preint : (1 0 0)
+ Ftr4 trk0 ProcessorOdom2d <--
+ m = ( 1 0 0 )
+ Fac4 FactorOdom2d --> Frm1
+ Cap7 CaptureRangeBearing -> Sen2 <--
+ Ftr5 trk0 FeatureRangeBearing <--
+ m = ( 1.4 2.4 )
+ Fac5 RANGE BEARING --> Lmk1
+ Ftr6 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac6 RANGE BEARING --> Lmk2
+ KFrm3 <--
+ P[Est] = ( 2 1.2e-08 )
+ O[Est] = ( 6.6e-09 )
+ CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <--
+ buffer size : 2
+ delta preint : (1 0 0)
+ Ftr7 trk0 ProcessorOdom2d <--
+ m = ( 1 0 0 )
+ Fac7 FactorOdom2d --> Frm2
+ Cap9 CaptureRangeBearing -> Sen2 <--
+ Ftr8 trk0 FeatureRangeBearing <--
+ m = ( 1.4 2.4 )
+ Fac8 RANGE BEARING --> Lmk2
+ Ftr9 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac9 RANGE BEARING --> Lmk3
+ Frm4 <--
+ P[Est] = ( 2 0 )
+ O[Est] = ( 0 )
+ CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <--
+ buffer size : 1
+ delta preint : (0 0 0)
+ Map
+ Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by factors 3 & 5
+ P[Est] = ( 1 2 )
+ Lmk2 LandmarkPoint2d <-- Fac6 Fac8
+ P[Est] = ( 2 2 )
+ Lmk3 LandmarkPoint2d <-- Fac9
+ P[Est] = ( 3 2 )
+ -----------------------------------------
*
* ============= GENERAL WOLF NOTES ==================
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 42dc859ad9753c44b184c3a8b1e77548fcc135c0..36e5e338bde88d49062931e383929baa005bc9df 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -50,6 +50,7 @@ class Problem : public std::enable_shared_from_this<Problem>
friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
friend ProcessorBase;
friend ProcessorMotion;
+ friend IsMotion;
protected:
TreeManagerBasePtr tree_manager_;
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index 8c99fe72e37698a2210e35a3af0c14bf9168f65e..3ba3f97ad15249f425e357b7901a1494a5e56073 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -45,7 +45,8 @@ class IsMotion
std::string getStateStructure(){return state_structure_;};
void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;};
-
+ void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr);
+
protected:
std::string state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
@@ -83,8 +84,6 @@ inline Eigen::VectorXd IsMotion::getState(const TimeStamp& _ts) const
return x;
}
-
-
} /* namespace wolf */
#endif /* PROCESSOR_IS_MOTION_H_ */
diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1bfd06a5e9669f76ef48c49a559cade0dc7845f0
--- /dev/null
+++ b/src/processor/is_motion.cpp
@@ -0,0 +1,9 @@
+#include "core/processor/is_motion.h"
+#include "core/problem/problem.h"
+
+using namespace wolf;
+
+void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
+{
+ _prb_ptr->addProcessorIsMotion(_motion_ptr);
+}
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 4bce5f885a2104ee45c646e3a2a4e45a9d840b49..457394c9213b39a69da4a01f74f2804db723a573 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -41,22 +41,17 @@ void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
Eigen::MatrixXd& _jacobian_calib) const
{
assert((_data.size() == 6 || _data.size() == 7) && "Wrong data size. Must be 6 or 7 for 3d.");
- double disp, rot; // displacement and rotation of this motion step
if (_data.size() == (long int)6)
{
// rotation in vector form
_delta.head<3>() = _data.head<3>();
Eigen::Map<Eigen::Quaterniond> q(_delta.data() + 3);
q = v2q(_data.tail<3>());
- disp = _data.head<3>().norm();
- rot = _data.tail<3>().norm();
}
else
{
// rotation in quaternion form
_delta = _data;
- disp = _data.head<3>().norm();
- rot = 2.0 * acos(_data(6)); // '6' is the real part of the quaternion
}
/* Jacobians of d = data2delta(data, dt)
* with: d = [Dp Dq]
@@ -72,13 +67,7 @@ void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
*
* so, J = I, and delta_cov = _data_cov
*/
- // We discard _data_cov and create a new one from the measured motion
- double disp_var = min_disp_var_ + k_disp_to_disp_ * disp;
- double rot_var = min_rot_var_ + k_disp_to_rot_ * disp + k_rot_to_rot_ * rot;
- Eigen::Matrix6d data_cov(Eigen::Matrix6d::Identity());
- data_cov(0, 0) = data_cov(1, 1) = data_cov(2, 2) = disp_var;
- data_cov(3, 3) = data_cov(4, 4) = data_cov(5, 5) = rot_var;
- _delta_cov = data_cov;
+ _delta_cov = _data_cov;
}
void ProcessorOdom3d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 2696310df9b49f0cd621a7e3218cbc1bbaf72998..f326eb329e7a1b687acf258352df211ea79ce166 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -102,7 +102,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// No-break case only for debug. Next case will be executed too.
PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
- } // @suppress("No break at end of case")
+ }
+ // Fall through
case SECOND_TIME_WITHOUT_PACK :
{
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 3862e263cb747b443e77ef113e9d85efa529f3a5..b2fc65a6acea79636728737980f8d18818524ee8 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -137,7 +137,9 @@ TEST_F(BufferPackKeyFrameTest, selectPack)
{
PackKeyFramePtr packQ = pack_kf_buffer.selectPack(16, q[iq]);
if (packQ!=nullptr)
+ {
ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
+ }
}
pack_kf_buffer.clear();
}