Skip to content
Snippets Groups Projects
Commit ce27ff02 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Merge branch 'devel' into Apriltag

parents c644f85a e5926ef6
No related branches found
No related tags found
No related merge requests found
Showing
with 142 additions and 30 deletions
......@@ -104,7 +104,7 @@ int main()
using namespace wolf;
// Wolf problem and solver
ProblemPtr problem = Problem::create("PO 2D");
ProblemPtr problem = Problem::create("PO", 2);
ceres::Solver::Options options;
options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options);
......
......@@ -47,7 +47,8 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
// 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture);
kf->addCapture(capture_rb);
// kf->addCapture(capture_rb);
capture_rb->link(kf);
// 3. explore all observations in the capture
for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
......@@ -70,9 +71,10 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
{
// new landmark:
// - create landmark
lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing));
// lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing));
lmk = std::static_pointer_cast<LandmarkPoint2D>(LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing)));
WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
getProblem()->getMap()->addLandmark(lmk);
// getProblem()->getMap()->addLandmark(lmk);
// - add to known landmarks
known_lmks.emplace(id, lmk);
}
......@@ -81,17 +83,23 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
Vector2s rb(range,bearing);
auto ftr = std::make_shared<FeatureRangeBearing>(rb,
getSensor()->getNoiseCov());
capture_rb->addFeature(ftr);
// capture_rb->addFeature(ftr);
FeatureBase::emplace<FeatureRangeBearing>(capture_rb, rb, getSensor()->getNoiseCov());
// 6. create factor
auto prc = shared_from_this();
auto ctr = std::make_shared<FactorRangeBearing>(capture_rb,
lmk,
prc,
false,
FAC_ACTIVE);
ftr->addFactor(ctr);
lmk->addConstrainedBy(ctr);
// auto ctr = std::make_shared<FactorRangeBearing>(capture_rb,
// lmk,
// prc,
// false,
// FAC_ACTIVE);
auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb,
lmk,
prc,
false,
FAC_ACTIVE);
// ftr->addFactor(ctr);
// lmk->addConstrainedBy(ctr);
}
}
......
......@@ -99,6 +99,9 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
SizeEigen getCalibSize() const;
virtual Eigen::VectorXs getCalibration() const;
void setCalibration(const Eigen::VectorXs& _calib);
void link(FrameBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<CaptureBase> emplace(FrameBasePtr _frm_ptr, T&&... all);
protected:
SizeEigen computeCalibSize() const;
......@@ -116,6 +119,14 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
namespace wolf{
template<typename classType, typename... T>
std::shared_ptr<CaptureBase> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all)
{
CaptureBasePtr cpt = std::make_shared<classType>(std::forward<T>(all)...);
cpt->link(_frm_ptr);
return cpt;
}
inline SizeEigen CaptureBase::getCalibSize() const
{
return calib_size_;
......
......@@ -81,7 +81,6 @@ class NodeBase
void setType(const std::string& _type);
void setName(const std::string& _name);
ProblemPtr getProblem() const;
virtual void setProblem(ProblemPtr _prob_ptr);
};
......
......@@ -171,6 +171,10 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
*/
void setProcessor(const ProcessorBasePtr& _processor_ptr);
void link(FeatureBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all);
// protected:
// template<typename D>
// void print(const std::string& name, const Eigen::MatrixBase<D>& mat) const; // Do nothing if input Scalar type is ceres::Jet
......@@ -209,6 +213,15 @@ namespace wolf{
// }
//}
template<typename classType, typename... T>
std::shared_ptr<FactorBase> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all)
{
FactorBasePtr ctr = std::make_shared<classType>(std::forward<T>(all)...);
ctr->link(_ftr_ptr);
return ctr;
}
inline unsigned int FactorBase::id() const
{
return factor_id_;
......
......@@ -95,6 +95,10 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
// all factors
void getFactorList(FactorBasePtrList & _fac_list);
void link(CaptureBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<FeatureBase> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
private:
Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
};
......@@ -107,6 +111,14 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
namespace wolf{
template<typename classType, typename... T>
std::shared_ptr<FeatureBase> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
{
FeatureBasePtr ftr = std::make_shared<classType>(std::forward<T>(all)...);
ftr->link(_cpt_ptr);
return ftr;
}
inline unsigned int FeatureBase::getHits() const
{
return constrained_by_list_.size();
......
......@@ -65,6 +65,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
**/
FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x);
virtual ~FrameBase();
virtual void remove();
......@@ -144,6 +146,9 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
FactorBasePtrList& getConstrainedByList();
void link(TrajectoryBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<FrameBase> emplace(TrajectoryBasePtr _ptr, T&&... all);
public:
static FrameBasePtr create_PO_2D (const FrameType & _tp,
......@@ -168,6 +173,14 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
namespace wolf {
template<typename classType, typename... T>
std::shared_ptr<FrameBase> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all)
{
FrameBasePtr frm = std::make_shared<classType>(std::forward<T>(all)...);
frm->link(_ptr);
return frm;
}
inline unsigned int FrameBase::id()
{
return frame_id_;
......
......@@ -44,7 +44,8 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
* \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
*
**/
LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
LandmarkBase(MapBaseWPtr _ptr, const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
virtual ~LandmarkBase();
virtual void remove();
virtual YAML::Node saveToYaml() const;
......@@ -78,7 +79,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
// Descriptor
public:
const Eigen::VectorXs& getDescriptor() const;
const Eigen::VectorXs& getDescriptor() const;
Scalar getDescriptor(unsigned int _ii) const;
void setDescriptor(const Eigen::VectorXs& _descriptor);
......@@ -91,6 +92,9 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
void setMap(const MapBasePtr _map_ptr);
MapBasePtr getMap();
void link(MapBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<LandmarkBase> emplace(MapBasePtr _map_ptr, T&&... all);
};
......@@ -102,6 +106,13 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
namespace wolf{
template<typename classType, typename... T>
std::shared_ptr<LandmarkBase> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all)
{
LandmarkBasePtr lmk = std::make_shared<classType>(std::forward<T>(all)...);
lmk->link(_map_ptr);
return lmk;
}
inline void LandmarkBase::setProblem(ProblemPtr _problem)
{
NodeBase::setProblem(_problem);
......
......@@ -49,19 +49,22 @@ class Problem : public std::enable_shared_from_this<Problem>
mutable std::mutex mut_state_block_notifications_;
mutable std::mutex mut_covariances_;
bool prior_is_set_;
std::string frame_structure_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(const std::string& _frame_structure); // USE create() below !!
Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !!
void setup();
public:
static ProblemPtr create(const std::string& _frame_structure); // USE THIS AS A CONSTRUCTOR!
static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
virtual ~Problem();
// Properties -----------------------------------------
SizeEigen getFrameStructureSize() const;
void getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const;
SizeEigen getDim() const;
std::string getFrameStructure() const;
// Hardware branch ------------------------------------
HardwareBasePtr getHardware();
......@@ -140,6 +143,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief Emplace frame from string frame_structure
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _frame_state State vector; must match the size and format of the chosen frame structure
* \param _time_stamp Time stamp of the frame
......@@ -150,12 +154,14 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
FrameType _frame_key_type, //
const Eigen::VectorXs& _frame_state, //
const TimeStamp& _time_stamp);
/** \brief Emplace frame from string frame_structure without state
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
*
......@@ -165,6 +171,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
FrameType _frame_key_type, //
const TimeStamp& _time_stamp);
......
......@@ -210,6 +210,9 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void setVotingActive(bool _voting_active = true);
void link(SensorBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<ProcessorBase> emplace(SensorBasePtr _sen_ptr, T&&... all);
void setVotingAuxActive(bool _voting_active = true);
};
......@@ -240,6 +243,14 @@ inline void ProcessorBase::setVotingAuxActive(bool _voting_active)
namespace wolf {
template<typename classType, typename... T>
std::shared_ptr<ProcessorBase> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
{
ProcessorBasePtr prc = std::make_shared<classType>(std::forward<T>(all)...);
prc->link(_sen_ptr);
return prc;
}
inline bool ProcessorBase::isMotion()
{
return false;
......
......@@ -180,6 +180,9 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
Eigen::MatrixXs getNoiseCov();
void setExtrinsicDynamic(bool _extrinsic_dynamic);
void setIntrinsicDynamic(bool _intrinsic_dynamic);
void link(HardwareBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<SensorBase> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
protected:
SizeEigen computeCalibSize() const;
......@@ -197,6 +200,14 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
namespace wolf{
template<typename classType, typename... T>
std::shared_ptr<SensorBase> SensorBase::emplace(HardwareBasePtr _hwd_ptr, T&&... all)
{
SensorBasePtr sen = std::make_shared<classType>(std::forward<T>(all)...);
sen->link(_hwd_ptr);
return sen;
}
inline unsigned int SensorBase::id()
{
return sensor_id_;
......
......@@ -25,10 +25,11 @@ CaptureGPSFix::~CaptureGPSFix()
void CaptureGPSFix::process()
{
// EXTRACT AND ADD FEATURES
addFeature(std::make_shared<FeatureGPSFix>(data_,data_covariance_));
// addFeature(std::make_shared<FeatureGPSFix>(data_,data_covariance_));
// ADD CONSTRAINT
getFeatureList().front()->addFactor(std::make_shared <FactorGPS2D>(getFeatureList().front()));
// getFeatureList().front()->addFactor(std::make_shared <FactorGPS2D>(getFeatureList().front()));
FactorBase::emplace<FactorGPS2D>(getFeatureList().front(), getFeatureList().front());
}
} //namespace wolf
......@@ -99,8 +99,6 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
{
//std::cout << "Adding feature" << std::endl;
feature_list_.push_back(_ft_ptr);
_ft_ptr->setCapture(shared_from_this());
_ft_ptr->setProblem(getProblem());
return _ft_ptr;
}
......@@ -294,5 +292,20 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
}
}
void CaptureBase::link(FrameBasePtr _frm_ptr)
{
if(_frm_ptr)
{
_frm_ptr->addCapture(shared_from_this());
this->setFrame(_frm_ptr);
this->setProblem(_frm_ptr->getProblem());
this->registerNewStateBlocks();
}
else
{
WOLF_WARN("Linking with a nullptr");
}
}
} // namespace wolf
......@@ -18,14 +18,16 @@ CapturePose::~CapturePose()
void CapturePose::emplaceFeatureAndFactor()
{
// Emplace feature
FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_);
addFeature(feature_pose);
// FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_);
// addFeature(feature_pose);
auto feature_pose = FeatureBase::emplace<FeaturePose>(shared_from_this(), data_, data_covariance_);
std::cout << data_.size() << " ~~ " << data_covariance_.rows() << "x" << data_covariance_.cols() << std::endl;
// Emplace factor
if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
feature_pose->addFactor(std::make_shared<FactorPose2D>(feature_pose));
FactorBase::emplace<FactorPose2D>(feature_pose, feature_pose);
else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
feature_pose->addFactor(std::make_shared<FactorPose3D>(feature_pose));
FactorBase::emplace<FactorPose3D>(feature_pose, feature_pose);
else
throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
}
......
......@@ -163,7 +163,7 @@ int main(int argc, char** argv)
}
// Wolf problem
ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D");
ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2);
const std::string sensor_name("Main Odometer");
Eigen::VectorXs extrinsics(3);
......
......@@ -21,7 +21,7 @@ int main()
//=====================================================
// Wolf problem
ProblemPtr wolf_problem_ptr_ = Problem::create("PO 3D");
ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 3);
/* Do this while there aren't extrinsic parameters on the yaml */
Eigen::Vector7s extrinsic_cam;
......
......@@ -52,7 +52,7 @@ int main(int argc, char** argv)
std::string wolf_root = _WOLF_ROOT_DIR;
std::cout << "Wolf root: " << wolf_root << std::endl;
ProblemPtr wolf_problem_ = Problem::create("PO 3D");
ProblemPtr wolf_problem_ = Problem::create("PO", 3);
//=====================================================
// Method 1: Use data generated here for sensor and processor
......
......@@ -16,7 +16,7 @@ int main()
using namespace Eigen;
using namespace std;
ProblemPtr problem = Problem::create("PO 2D");
ProblemPtr problem = Problem::create("PO", 2);
SensorBasePtr sen_odo = problem->installSensor ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),"");
ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
......
......@@ -75,7 +75,7 @@ int main()
std::string wolf_config = wolf_root + "/src/examples";
std::cout << "\nWolf directory for configuration files: " << wolf_config << std::endl;
ProblemPtr problem = Problem::create("PO 2D");
ProblemPtr problem = Problem::create("PO", 2);
filename = wolf_config + "/map_polyline_example.yaml";
std::cout << "Reading map from file: " << filename << std::endl;
problem->loadMap(filename);
......
......@@ -40,7 +40,7 @@ int main (int argc, char** argv)
}
cout << "Final timestamp tf = " << tf.get() << " s" << endl;
ProblemPtr problem = Problem::create("PO 3D");
ProblemPtr problem = Problem::create("PO", 3);
ceres::Solver::Options ceres_options;
// ceres_options.max_num_iterations = 1000;
// ceres_options.function_tolerance = 1e-10;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment