diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 58aab197f8174e67b584742198ea2ce8fd88e1cb..249b84e5c1902743a60b5ad0665318b4b622bc1a 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -88,8 +88,6 @@ license_header:
build_and_test:bionic:
stage: build_and_test
image: labrobotica/wolf_deps:18.04
- except:
- - master
script:
- *build_and_test_definition
@@ -97,8 +95,6 @@ build_and_test:bionic:
build_and_test:focal:
stage: build_and_test
image: labrobotica/wolf_deps:20.04
- except:
- - master
script:
- *build_and_test_definition
@@ -110,6 +106,7 @@ deploy_imu:
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
+ branch: $WOLF_IMU_BRANCH
strategy: depend
deploy_gnss:
@@ -119,6 +116,7 @@ deploy_gnss:
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
+ branch: $WOLF_GNSS_BRANCH
strategy: depend
deploy_vision:
@@ -128,6 +126,7 @@ deploy_vision:
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
+ branch: $WOLF_VISION_BRANCH
strategy: depend
deploy_laser:
@@ -137,6 +136,7 @@ deploy_laser:
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
+ branch: $WOLF_LASER_BRANCH
strategy: depend
deploy_apriltag:
@@ -146,6 +146,7 @@ deploy_apriltag:
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
+ branch: $WOLF_APRILTAG_BRANCH
strategy: depend
deploy_bodydynamics:
@@ -155,6 +156,7 @@ deploy_bodydynamics:
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
+ branch: $WOLF_BODYDYNAMICS_BRANCH
strategy: depend
############ WAIT FOR PLUGINS ############
@@ -177,7 +179,6 @@ deploy_wolf_ros_node:
WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
WOLF_APRILTAG_BRANCH: $WOLF_APRILTAG_BRANCH
WOLF_BODYDYNAMICS_BRANCH: $WOLF_BODYDYNAMICS_BRANCH
- WOLF_ROS_CORE_BRANCH: $WOLF_ROS_CORE_BRANCH
WOLF_ROS_IMU_BRANCH: $WOLF_ROS_IMU_BRANCH
WOLF_ROS_GNSS_BRANCH: $WOLF_ROS_GNSS_BRANCH
WOLF_ROS_LASER_BRANCH: $WOLF_ROS_LASER_BRANCH
@@ -186,6 +187,7 @@ deploy_wolf_ros_node:
WOLF_ROS_BODYDYNAMICS_BRANCH: $WOLF_ROS_BODYDYNAMICS_BRANCH
trigger:
project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node
+ branch: $WOLF_ROS_CORE_BRANCH
no_deploy_wolf_ros_node:
stage: deploy_ros
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index db934ca13174443de5053e3a427206de41799c18..27214245096f91fefad1351f611613516010f40a 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -138,8 +138,8 @@ int main()
SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server);
// recover sensor pointers and other stuff for later use (access by sensor name)
- SensorBasePtr sensor_odo = problem->getSensor("sen odom");
- SensorBasePtr sensor_rb = problem->getSensor("sen rb");
+ SensorBasePtr sensor_odo = problem->findSensor("sen odom");
+ SensorBasePtr sensor_rb = problem->findSensor("sen rb");
// APPLY PRIOR and SET PROCESSOR ODOM ORIGIN ===================================================
TimeStamp t(0.0);
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 7405ae8320deb339ed6efdfe7319b4df8ec14cff..08a7ab52fd0598315adf2b9150ad80efd77043d0 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -151,7 +151,11 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
- SensorBasePtr getSensor(const std::string& _sensor_name) const;
+ SensorBasePtr findSensor(const std::string& _sensor_name) const;
+ /** \brief get a processor pointer by its name
+ * \param _processor_name The processor name, as it was installed with installProcessor()
+ */
+ ProcessorBasePtr findProcessor(const std::string& _processor_name) const;
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index 87511440e721baf20d2b3aa05123f0499caa4c84..e60a59f8233abb7fade714f20cbabc417a76e091 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -119,11 +119,13 @@ class TrackMatrix
FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap) const;
CaptureBasePtr firstCapture(const SizeStd& _track_id) const;
+ list<size_t> trackIds() const;
+
// tracks across captures that belong to keyframe
-// SizeStd numKeyframeTracks();
Track trackAtKeyframes(size_t _track_id) const;
-// bool markKeyframe(CaptureBasePtr _capture);
-// bool unmarkKeyframe(CaptureBasePtr _capture);
+
+ const map<SizeStd, Track>& getTracks() const {return tracks_;}
+ const map<CaptureBasePtr, Snapshot >& getSnapshots() const {return snapshots_;}
private:
@@ -131,8 +133,6 @@ class TrackMatrix
// tracks across all Captures
map<SizeStd, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
-// // tracks across captures that belong to keyframe
-// map<size_t, Track > tracks_kf_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
// Across track: maps of Feature pointers indexed by track_Id.
map<CaptureBasePtr, Snapshot > snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) )
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 4200f81f7e3faac814a45bea8995a13436b6b7c3..f22772908c411fba8a096fb764af40fe6b39316e 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -51,12 +51,19 @@ class StateQuaternion : public StateBlock
inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
{
+ // TODO: remove these lines after issue #381 is addressed
+ assert(isValid(1e-5) && "Quaternion unit norm is worse than 1e-5 tolerance!");
+ state_.normalize();
}
inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed) :
StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
{
- assert(_state.size() == 4 && "The quaternion state vector must be of size 4");
+ assert(state_.size() == 4 && "The quaternion state vector must be of size 4");
+
+ // TODO: remove these lines after issue #381 is addressed
+ assert(isValid(1e-5) && "Quaternion unit norm is worse than 1e-5 tolerance!");
+ state_.normalize();
}
inline StateQuaternion::StateQuaternion(bool _fixed) :
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 11db6b079bd8371a48a660547997f7c68d3198bf..5e3516cccc2f6095e2c4c8905ceb90649cfeda9e 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -198,7 +198,7 @@ void FeatureBase::link(CaptureBasePtr _cpt_ptr)
}
void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "c " : "");
+ _stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "fac " : "");
if (_constr_by)
{
_stream << " <--\t";
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 1e5c88a2db6cf68a2032c8be1b50dbbe3c2206d9..d80dafb51c2d4e2dfd772c19fa0786e4f5c173b7 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -300,7 +300,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _corresponding_sensor_name, //
const std::string& _params_filename)
{
- SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
+ SensorBasePtr sen_ptr = findSensor(_corresponding_sensor_name);
if (sen_ptr == nullptr)
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
if (_params_filename == "")
@@ -318,7 +318,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _corresponding_sensor_name, //
const ParamsServer& _server)
{
- SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
+ SensorBasePtr sen_ptr = findSensor(_corresponding_sensor_name);
if (sen_ptr == nullptr)
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
@@ -335,7 +335,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
return prc_ptr;
}
-SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
+SensorBasePtr Problem::findSensor(const std::string& _sensor_name) const
{
auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
getHardware()->getSensorList().end(),
@@ -350,6 +350,16 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
return (*sen_it);
}
+ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name) const
+{
+ for (const auto& sensor : getHardware()->getSensorList())
+ for (const auto& processor : sensor->getProcessorList())
+ if (processor->getName() == _processor_name)
+ return processor;
+ return nullptr;
+}
+
+
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
const StateStructure& _frame_structure, //
const SizeEigen _dim, //
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index c707c37eadae74f631b392317c286fe057713f76..512d941b7669c13e22a87aee23c5a8f84ed44329 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -87,7 +87,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Process info
// TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do.
- // TrackerLandmark: If we have given a map, all landmarks in the map are know. Process them.
+ // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
processKnown();
// Update pointers
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index 1830270b13d7fea4d6462f2c97c7977b0879308f..d7154a788b5727b6e2f8cd201b382643110d5bf1 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -244,4 +244,14 @@ Track TrackMatrix::trackAtKeyframes(size_t _track_id) const
return Track();
}
+list<size_t> TrackMatrix::trackIds() const
+{
+ list<size_t> track_ids;
+ for (auto track : tracks_)
+ {
+ track_ids.push_back(track.first);
+ }
+ return track_ids;
+}
+
}
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index 33b358362e0247c570ccd6ae48d64bf7a89ef31b..6332a69928a558cfdede9125a73fd7ab7f835338 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -108,7 +108,7 @@ TEST(FactoryStateBlock, creator_StateBlock)
TEST(FactoryStateBlock, creator_Quaternion)
{
- auto sbq = FactoryStateBlock::create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
+ auto sbq = FactoryStateBlock::create("StateQuaternion", Eigen::Vector4d(1,2,3,4).normalized(), false);
ASSERT_EQ(sbq->getSize() , 4);
ASSERT_EQ(sbq->getLocalSize(), 3);
@@ -144,7 +144,7 @@ TEST(FactoryStateBlock, creator_H)
TEST(FactoryStateBlock, creator_O_is_quaternion)
{
- auto sbq = FactoryStateBlock::create("O", Eigen::Vector4d(1,2,3,4), false);
+ auto sbq = FactoryStateBlock::create("O", Eigen::Vector4d(1,2,3,4).normalized(), false);
ASSERT_EQ(sbq->getSize() , 4);
ASSERT_EQ(sbq->getLocalSize(), 3);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index d86942de20f525f0b0995e2a5b43e18750311697..80fe044c410c9b07f5e059461875a97199d1c6de 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -101,7 +101,7 @@ TEST(Problem, Installers)
{
std::string wolf_root = _WOLF_ROOT_DIR;
ProblemPtr P = Problem::create("PO", 3);
- Eigen::Vector7d xs;
+ Eigen::Vector7d xs; xs.setRandom(); xs.tail(4).normalize();
SensorBasePtr S = P->installSensor ("SensorOdom3d", "odometer", xs, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
@@ -183,7 +183,8 @@ TEST(Problem, SetOrigin_PO_3d)
{
ProblemPtr P = Problem::create("PO", 3);
TimeStamp t0(0);
- Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7; // not normalized quaternion, this is not what's tested
+ Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7;
+ vec7.tail(4).normalize();
VectorComposite x0(vec7, "PO", {3,4});
// Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
Eigen::VectorXd vec6(6); vec6 << sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1);
@@ -242,9 +243,13 @@ TEST(Problem, emplaceFrame_factory)
{
ProblemPtr P = Problem::create("PO", 2);
- FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2, VectorXd(3) );
- FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3, VectorXd(7) );
- FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3, VectorXd(10) );
+ Vector3d xpo2;
+ Vector7d xpo3; xpo3 << 1,2,3,.5,.5,.5,.5;
+ VectorXd xpov3(10); xpov3 << 1,2,3,.5,.5,.5,.5,1,2,3;
+
+ FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2, xpo2 );
+ FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3, xpo3 );
+ FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3, xpov3 );
// check that all frames are effectively in the trajectory
@@ -324,7 +329,8 @@ TEST(Problem, Covariances)
std::string wolf_root = _WOLF_ROOT_DIR;
ProblemPtr P = Problem::create("PO", 3);
- Eigen::Vector7d xs3d;
+ Eigen::Vector7d xs3d; xs3d.setRandom(); xs3d.tail(4).normalize();
+
Eigen::Vector3d xs2d;
SensorBasePtr Sm = P->installSensor ("SensorOdom3d", "odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index 4294d2ab663a7d7e171c3b663a6cf2575c6c5049..d37c700a7daf6a45862a4d8b830b8b8e67767282 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -38,7 +38,7 @@ TEST(Pose, constructor)
{
auto intr = std::make_shared<ParamsSensorPose>();
- Vector7d extr; extr << 1,2,3,4,5,6,7;
+ Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
auto sen = std::make_shared<SensorPose>(extr, intr);
@@ -54,7 +54,7 @@ TEST(Pose, getParams)
intr->std_p = 2;
intr->std_o = 3;
- Vector7d extr; extr << 1,2,3,4,5,6,7;
+ Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
auto sen = std::make_shared<SensorPose>(extr, intr);
@@ -70,7 +70,7 @@ TEST(Pose, create)
intr->std_p = 2;
intr->std_o = 3;
- Vector7d extr; extr << 1,2,3,4,5,6,7;
+ Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
auto sen_base = SensorPose::create("name", extr, intr);
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 914b8a7c3a989c52f86abbb2b8274732acf3aa2f..3d3ded934d762d21ba081779155f320db1b5327f 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -114,7 +114,8 @@ TEST(TreeManager, keyFrameCallback)
ASSERT_EQ(GM->n_KF_, 0);
- auto F0 = P->emplaceFrame(0, "PO", 3, VectorXd(7) );
+ Vector7d x; x << 1,2,3,0,0,0,1;
+ auto F0 = P->emplaceFrame(0, "PO", 3, x );
P->keyFrameCallback(F0, nullptr);
ASSERT_EQ(GM->n_KF_, 1);
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index e394a7c59279bb4d0e52c94b7eef479406ce5797..448fc0d93d27e1db1ee76c6ae46223be81a3c505 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -1050,11 +1050,11 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
TimeStamp t(0.0);
double dt = 1;
CaptureMotionPtr capture = std::make_shared<CaptureOdom3d>(t,
- problem->getSensor("odom"),
+ problem->findSensor("odom"),
data,
data_cov);
CaptureMotionPtr capture_bl = std::make_shared<CaptureOdom3d>(t,
- problem_bl->getSensor("odom"),
+ problem_bl->findSensor("odom"),
data,
data_cov);