Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/wolf
1 result
Show changes
Commits on Source (10)
Showing
with 167 additions and 165 deletions
......@@ -21,7 +21,7 @@ class CaptureRangeBearing : public CaptureBase
{
public:
CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings);
virtual ~CaptureRangeBearing();
~CaptureRangeBearing() override;
const VectorXi& getIds () const;
const int& getId (int _i) const;
......
......@@ -45,12 +45,12 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
//
}
virtual ~FactorRangeBearing()
~FactorRangeBearing() override
{
//
}
virtual std::string getTopology() const override
std::string getTopology() const override
{
return "LMK";
}
......
......@@ -19,7 +19,7 @@ class FeatureRangeBearing : public FeatureBase
{
public:
FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
virtual ~FeatureRangeBearing();
~FeatureRangeBearing() override;
};
} // namespace wolf
......
......@@ -19,7 +19,7 @@ class LandmarkPoint2d : public LandmarkBase
{
public:
LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy);
virtual ~LandmarkPoint2d();
~LandmarkPoint2d() override;
};
} /* namespace wolf */
......
......@@ -46,31 +46,31 @@ class ProcessorRangeBearing : public ProcessorBase
typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
ProcessorRangeBearing(ParamsProcessorBasePtr _params);
virtual ~ProcessorRangeBearing() {/* empty */}
virtual void configure(SensorBasePtr _sensor) override;
~ProcessorRangeBearing() override {/* empty */}
void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorRangeBearing, ParamsProcessorRangeBearing);
protected:
// Implementation of pure virtuals from ProcessorBase
virtual void processCapture (CaptureBasePtr _capture) override;
virtual void processKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
virtual bool triggerInCapture (CaptureBasePtr) const override { return true;};
virtual bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
virtual bool voteForKeyFrame () const override {return false;}
void processCapture (CaptureBasePtr _capture) override;
void processKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
bool triggerInCapture (CaptureBasePtr) const override { return true;};
bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
bool voteForKeyFrame () const override {return false;}
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
virtual bool storeKeyFrame(FrameBasePtr) override;
bool storeKeyFrame(FrameBasePtr) override;
/** \brief store capture
*
* Returns true if the capture should be stored
*/
virtual bool storeCapture(CaptureBasePtr) override;
bool storeCapture(CaptureBasePtr) override;
private:
// control variables
......
......@@ -45,7 +45,7 @@ class SensorRangeBearing : public SensorBase
SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std);
SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr);
WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing, 3);
virtual ~SensorRangeBearing();
~SensorRangeBearing() override;
};
......
......@@ -35,7 +35,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
protected:
unsigned int capture_id_;
TimeStamp time_stamp_; ///< Time stamp
virtual void setProblem(ProblemPtr _problem) final;
void setProblem(ProblemPtr _problem) override final;
public:
......@@ -46,7 +46,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _intr_ptr = nullptr);
virtual ~CaptureBase();
~CaptureBase() override;
virtual void remove(bool viral_remove_empty_parent=false);
// Type
......@@ -89,8 +89,8 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
StateBlockPtr getSensorO() const;
StateBlockPtr getSensorIntrinsic() const;
virtual void fix() override;
virtual void unfix() override;
void fix() override;
void unfix() override;
void move(FrameBasePtr);
void link(FrameBasePtr);
......
......@@ -37,7 +37,7 @@ public:
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _intr_ptr = nullptr);
virtual ~CaptureDiffDrive() = default;
~CaptureDiffDrive() override = default;
};
} // namespace wolf
......
......@@ -62,10 +62,10 @@ class CaptureMotion : public CaptureBase
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _intr_ptr = nullptr);
virtual ~CaptureMotion();
~CaptureMotion() override;
// Type
virtual bool isMotion() const override { return true; }
bool isMotion() const override { return true; }
// Data
const Eigen::VectorXd& getData() const;
......@@ -96,7 +96,7 @@ class CaptureMotion : public CaptureBase
bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance);
virtual void printHeader(int depth, //
void printHeader(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
......
......@@ -31,7 +31,7 @@ class CaptureOdom2d : public CaptureMotion
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr = nullptr);
virtual ~CaptureOdom2d();
~CaptureOdom2d() override;
};
......
......@@ -31,7 +31,7 @@ class CaptureOdom3d : public CaptureMotion
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr = nullptr);
virtual ~CaptureOdom3d();
~CaptureOdom3d() override;
};
......
......@@ -25,7 +25,7 @@ class CapturePose : public CaptureBase
CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_covariance);
virtual ~CapturePose();
~CapturePose() override;
virtual void emplaceFeatureAndFactor();
......
......@@ -14,7 +14,7 @@ class CaptureVoid : public CaptureBase
{
public:
CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
virtual ~CaptureVoid();
~CaptureVoid() override;
};
......
......@@ -30,7 +30,7 @@ struct CeresParams : public SolverParams
//
}
virtual ~CeresParams() = default;
~CeresParams() override = default;
};
/** \brief Ceres manager for WOLF
*
......@@ -56,7 +56,7 @@ class CeresManager : public SolverManager
const ceres::Solver::Options& _ceres_options
= ceres::Solver::Options());
~CeresManager();
~CeresManager() override;
static SolverManagerPtr create(const ProblemPtr& _wolf_problem, const ParamsServer& _server);
......@@ -64,18 +64,18 @@ class CeresManager : public SolverManager
std::unique_ptr<ceres::Problem>& getCeresProblem();
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
void computeCovariances(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) override;
void computeCovariances(const std::vector<StateBlockPtr>& st_list) override;
virtual bool hasConverged() override;
bool hasConverged() override;
virtual SizeStd iterations() override;
SizeStd iterations() override;
virtual double initialCost() override;
double initialCost() override;
virtual double finalCost() override;
double finalCost() override;
ceres::Solver::Options& getSolverOptions();
......@@ -84,27 +84,27 @@ class CeresManager : public SolverManager
protected:
virtual bool checkDerived(std::string prefix="") const override;
bool checkDerived(std::string prefix="") const override;
virtual std::string solveDerived(const ReportVerbosity report_level) override;
std::string solveDerived(const ReportVerbosity report_level) override;
virtual void addFactorDerived(const FactorBasePtr& fac_ptr) override;
void addFactorDerived(const FactorBasePtr& fac_ptr) override;
virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override;
bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override;
};
inline ceres::Solver::Summary CeresManager::getSummary()
......
......@@ -25,9 +25,9 @@ class CostFunctionWrapper : public ceres::CostFunction
CostFunctionWrapper(FactorBasePtr _factor_ptr);
virtual ~CostFunctionWrapper();
~CostFunctionWrapper() override;
virtual bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const;
bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const override;
FactorBasePtr getFactor() const;
};
......
......@@ -21,15 +21,15 @@ class LocalParametrizationWrapper : public ceres::LocalParameterization
LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr);
virtual ~LocalParametrizationWrapper() = default;
~LocalParametrizationWrapper() override = default;
virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const;
bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const override;
virtual bool ComputeJacobian(const double* x, double* jacobian) const;
bool ComputeJacobian(const double* x, double* jacobian) const override;
virtual int GlobalSize() const;
int GlobalSize() const override;
virtual int LocalSize() const;
int LocalSize() const override;
LocalParametrizationBasePtr getLocalParametrization() const;
};
......
......@@ -126,18 +126,20 @@ namespace wolf
*
* Second to know, the Factory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>:
* it can only exist once in your application.
* To access it, use the static method get(),
* To access it, we internally use the static method get(),
*
* \code
* Factory<MyTypeBase, MyTypeInput>::get()
* \endcode
*
* where, of course, you better make use of the appropriate typedef in place of ````Factory<MyTypeBase, MyTypeInput>````.
* but this is a private method.
*
* You can then call the methods you like, e.g. to create a landmark, you use:
* The interesting methods are designed static, and are the ones responsible for accessing the Factory instance via .get().
*
* You must therefore call the methods you like statically, e.g. to create a landmark, you use:
*
* \code
* FactoryLandmark::get().create(TYPE, args...); // see below for creating objects ...
* FactoryLandmark::create(TYPE, args...); // see below for creating objects ...
* \endcode
*
* #### Write creator methods (in your derived object classes)
......@@ -172,7 +174,7 @@ namespace wolf
* that knows how to create your specific object, e.g.:
*
* \code
* FactoryLandmark::get().registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
* FactoryLandmark::registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
* \endcode
*
* #### Automatic registration
......@@ -180,7 +182,7 @@ namespace wolf
* For example, in sensor_camera_yaml.cpp we find the line:
*
* \code
* const bool registered_camera_intr = FactoryParamsSensor::get().registerCreator("ParamsSensorCamera", createParamsSensorCamera);
* const bool registered_camera_intr = FactoryParamsSensor::registerCreator("ParamsSensorCamera", createParamsSensorCamera);
* \endcode
*
* which is a static invocation (i.e., it is placed at global scope outside of the ParamsSensorCamera class).
......@@ -191,7 +193,7 @@ namespace wolf
* The method unregisterCreator() unregisters the ObjectXxx::create() method. It only needs to be passed the string of the object type.
*
* \code
* Factory<MyTypeBase, MyTypeInput>::get().unregisterCreator("ParamsSensorCamera");
* Factory<MyTypeBase, MyTypeInput>::unregisterCreator("ParamsSensorCamera");
* \endcode
*
* #### Create objects using the factory
......@@ -201,13 +203,13 @@ namespace wolf
* To create e.g. a LandmarkPolyline2d from a YAML node you type:
*
* \code
* LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::get().create("LandmarkPolyline2d", lmk_yaml_node);
* LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::create("LandmarkPolyline2d", lmk_yaml_node);
* \endcode
*
* or even better, make use of the convenient typedefs:
*
* \code
* LandmarkBasePtr lmk_ptr = FactoryLandmark::get().create("LandmarkPolyline2d", lmk_yaml_node);
* LandmarkBasePtr lmk_ptr = FactoryLandmark::create("LandmarkPolyline2d", lmk_yaml_node);
* \endcode
*
* ### Examples
......@@ -247,7 +249,7 @@ namespace wolf
* // Register landmark creator (put the register code inside an unnamed namespace):
* namespace
* {
* const bool registered_lmk_polyline_2d = FactoryLandmark::get().registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
* const bool registered_lmk_polyline_2d = FactoryLandmark::registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
* }
*
* \endcode
......@@ -280,9 +282,9 @@ class Factory
typedef std::map<std::string, CreatorCallback> CallbackMap;
public:
bool registerCreator(const std::string& _type, CreatorCallback createFn);
bool unregisterCreator(const std::string& _type);
TypeBasePtr create(const std::string& _type, TypeInput... _input);
static bool registerCreator(const std::string& _type, CreatorCallback createFn);
static bool unregisterCreator(const std::string& _type);
static TypeBasePtr create(const std::string& _type, TypeInput... _input);
std::string getClass() const;
private:
......@@ -291,7 +293,7 @@ class Factory
// Singleton ---------------------------------------------------
// This class is a singleton. The code below guarantees this.
// See: http://stackoverflow.com/questions/1008019/c-singleton-design-pattern
public:
private:
static Factory& get();
public:
Factory(const Factory&) = delete;
......@@ -304,11 +306,11 @@ class Factory
template<class TypeBase, typename... TypeInput>
inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn)
{
bool reg = callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
bool reg = get().callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
if (reg)
std::cout << std::setw(26) << std::left << getClass() << " <-- registered " << _type << std::endl;
std::cout << std::setw(26) << std::left << get().getClass() << " <-- registered " << _type << std::endl;
else
std::cout << std::setw(26) << std::left << getClass() << " X-- skipping " << _type << ": already registered." << std::endl;
std::cout << std::setw(26) << std::left << get().getClass() << " X-- skipping " << _type << ": already registered." << std::endl;
return reg;
}
......@@ -316,17 +318,17 @@ inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string&
template<class TypeBase, typename... TypeInput>
inline bool Factory<TypeBase, TypeInput...>::unregisterCreator(const std::string& _type)
{
return callbacks_.erase(_type) == 1;
return get().callbacks_.erase(_type) == 1;
}
template<class TypeBase, typename... TypeInput>
inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input)
{
typename CallbackMap::const_iterator creator_callback_it = callbacks_.find(_type);
typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
if (creator_callback_it == callbacks_.end())
if (creator_callback_it == get().callbacks_.end())
// not found
throw std::runtime_error(getClass() + " : Unknown type \"" + _type + "\". Possibly you tried to use an unregistered creator.");
throw std::runtime_error(get().getClass() + " : Unknown type \"" + _type + "\". Possibly you tried to use an unregistered creator.");
// Invoke the creation function
return (creator_callback_it->second)(std::forward<TypeInput>(_input)...);
......
......@@ -38,21 +38,21 @@ class FactorAnalytic: public FactorBase
StateBlockPtr _state8Ptr = nullptr,
StateBlockPtr _state9Ptr = nullptr );
virtual ~FactorAnalytic() = default;
~FactorAnalytic() override = default;
/** \brief Returns a vector of pointers to the states
*
* Returns a vector of pointers to the state in which this factor depends
*
**/
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
/** \brief Returns a vector of sizes of the state blocks
*
* Returns a vector of sizes of the state blocks
*
**/
virtual std::vector<unsigned int> getStateSizes() const override;
std::vector<unsigned int> getStateSizes() const override;
/** \brief Returns the factor residual size
*
......@@ -63,7 +63,7 @@ class FactorAnalytic: public FactorBase
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// load parameters evaluation value
std::vector<Eigen::Map<const Eigen::VectorXd>> state_blocks_map_;
......@@ -97,7 +97,7 @@ class FactorAnalytic: public FactorBase
/** Returns the residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
**/
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(_states_ptr.size() == state_block_sizes_vector_.size());
......@@ -152,7 +152,7 @@ class FactorAnalytic: public FactorBase
* Returns the jacobians computation method
*
**/
virtual JacobianMethod getJacobianMethod() const override;
JacobianMethod getJacobianMethod() const override;
private:
void resizeVectors();
......
This diff is collapsed.
......@@ -37,9 +37,9 @@ class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d,
{
}
virtual ~FactorAutodiffDistance3d() { /* nothing */ }
~FactorAutodiffDistance3d() override { /* nothing */ }
virtual std::string getTopology() const override
std::string getTopology() const override
{
return std::string("GEOM");
}
......