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 (6)
Showing
with 214 additions and 184 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;
};
......
......@@ -25,42 +25,68 @@ namespace wolf
/** \brief Singleton template factory
*
* This class implements a generic factory as a singleton.
*
* > IMPORTANT: This template factory can be used to construct many different objects except:
* > - Objects deriving from SensorBase --> see FactorySensor
* > - Objects deriving from ProcessorBase --> see FactoryProcessor
* >
* > The reason for this is that the two cases above need a more elaborated API than the one in this template class.
* This template class implements a generic factory as a singleton.
*
* \param TypeBase base type of all the objects created by the factory
* \param TypeInput type of the input argument. Typical cases are std::string for file names, and YAML::Node for YAML nodes.
* \param TypeInput variadic type for the input arguments.
*
* ### Template specialization
*
* - The class is templatized on the class of the produced objects, __TypeBase__.
* The produced objects are always of a class deriving from TypeBase.
* The returned data is always a pointer to TypeBase.
* The returned data is always a shared pointer to TypeBase.
*
* For example, you may use as __TypeBase__ the following types:
* - LandmarkBase: the Factory creates landmarks deriving from LandmarkBase and returns base pointers ````LandmarkBasePtr```` to them
* - ParamsSensorBase: the Factory creates intrinsic parameters deriving from ParamsSensorBase and returns base pointers ````ParamsSensorBasePtr```` to them
* - XxxBase: the Factory creates objects deriving from XxxBase and returns pointers ````XxxBasePtr```` to them.
*
* - The class in also templatized on the type of the input parameter of the creator, __TypeInput__:
* - ````std::string```` is used when the input parameter is a file name from which to read data (typically a YAML file).
* - ````YAML::Node```` is used when the input parameter is a YAML node with structured data.
* - The class is variadic-templatized on the types of the input parameter of the creator, __TypeInput__:
* - ````std::string```` is used when the input parameter is a file name from which to read data (typically a YAML file).
* - ````std::string, wolf::ParamsServer``` is used when nodes are build from parameters in the ParamsServer
* - ````YAML::Node```` is used when the input parameter is a YAML node with structured data.
* - Any other variadic list of inputs is possible.
*
* - Examples of specific factories existing in Wolf are:
* \code
*
* // FactorySensor
* typedef Factory<SensorBase, // Type of objects to be returned: SensorBasePtr
* const std::string&, // Type of the sensor: name of the derived sensor class
* const Eigen::VectorXd&, // Sensor extrinsics
* const ParamsSensorBasePtr> // Sensor parameters
* FactorySensor;
*
* // FactoryProcessor
* typedef Factory<ProcessorBase, // Type of object returned: ProcessorBasePtr
* const std::string&, // Type of the processor: name of the derived processor class
* const ParamsProcessorBasePtr> // Parameters for creating the processor
* FactoryProcessor;
*
* // AutoConfProcessorFactory
* typedef Factory<ProcessorBase, // Type of object returned: ProcessorBasePtr
* const std::string&, // Type of the processor: name of the derived processor class
* const ParamsServer> // Parameters for creating the processor
* AutoConfProcessorFactory;
*
* // Landmarks from YAML
* typedef Factory<LandmarkBase, // Type of node created: LandmarkBasePtr
* const YAML::Node&> // YAML node with the lmk params
* LandmarkFactory;
* \endcode
*
* ### Operation of the factory
*
* #### Rationale
*
* This factory can create objects of classes deriving from TypeBase.
* Once specialized, this factory can create objects of classes deriving from TypeBase.
*
* > For example, if you want to make a Landmark factory, set TypeBase = LandmarkBase.\n
* > Then, the factory will create specific landmarks deriving from LandmarkBase.\n
* > The specific type of landmark (e.g. LandmarkCorner2d, LandmarkAHP, LandmarkPolyline2d, etc) is indicated by a string that we call TYPE in this documentation.
*
* Specific object creation is invoked by the method ````create(TYPE, params ... )````, where
* - the TYPE of object to create is identified with a string
* - the TYPE of object to create is identified with a string. This string matches the name of the derived class.
* - the params may be provided in different forms -- see TypeInput.
*
* The methods to create specific objects are called __creators__.
......@@ -75,23 +101,31 @@ namespace wolf
* - Examples: Write and register a landmark creator for LandmarkPolyline2d.
*
* #### Define correct TYPE names
* The rule to make new TYPE strings unique is that you skip the generic 'Type' prefix from your class name,
* and you build a string in CAPITALS with space separators, e.g.:
* - ParamsSensorCamera -> ````"CAMERA"````
* - ParamsSensorLaser2d -> ````"LASER 2d"````
* - LandmarkPolyline2d -> ````"POLYLINE 2d"````
* We use a std::string with literally the same text as the derived class name, e.g.:
* - ParamsSensorCamera -> ````"ParamsSensorCamera"````
* - SensorCamera -> ````"SensorCamera"````
* - ParamsSensorLaser2d -> ````"ParamsSensorLaser2d"````
* - SensorLaser2d -> ````"SensorLaser2d"````
* - LandmarkPolyline2d -> ````"LandmarkPolyline2d"````
* - etc.
*
* #### Access the factory
*
* The first thing to know is that we have defined typedefs for the templates that we are using. For example:
*
* \code
* typedef Factory<ParamsSensorBase, std::string> FactoryParamsSensor;
* typedef Factory<ParamsProcessorBase, std::string> FactoryParamsProcessor;
* typedef Factory<LandmarkBase, YAML::Node> FactoryLandmark;
* typedef Factory<ParamsSensorBase, std::string> FactoryParamsSensor;
* typedef Factory<ParamsProcessorBase, std::string> FactoryParamsProcessor;
* typedef Factory<LandmarkBase, YAML::Node> FactoryLandmark;
* typedef Factory<SensorBase,
* const std::string&,
* const Eigen::VectorXd&,
* const ParamsSensorBasePtr> FactorySensor;
*
* \endcode
*
* 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.
* 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(),
*
* \code
......@@ -103,10 +137,11 @@ namespace wolf
* You can then call the methods you like, e.g. to create a landmark, you use:
*
* \code
* FactoryLandmark::get().create(...); // see below for creating objects ...
* FactoryLandmark::get().create(TYPE, args...); // see below for creating objects ...
* \endcode
*
* #### Write creator methods (in your derived object classes)
*
* The method LandmarkPolyline2d::create(...) exists in the LandmarkPolyline2d class as a static method.
* All these ````XxxXxx::create()```` methods need to have exactly the same API, regardless of the object type.
* The API puts into play the two template parameters:
......@@ -117,13 +152,13 @@ namespace wolf
*
* This API includes an element of type TypeInput, which might be either a std::string, or a YAML::node:
* - ````std::string```` is used to indicate the name of a configuration file. These files are usually YAML files containing configuration data to create your object.
* - ````YAML::Node```` is used to access parts of a YAML file already encoded as nodes, such as when loading landmarks from a SLAM map stored as a YAML file.
* - ````YAML::Node```` is used to access parts of a YAML file already encoded as nodes, such as when loading landmarks from a SLAM map stored as a YAML file.
*
* Two examples:
*
* \code
* static ParamsSensorBasePtr create(const std::string& _intrinsics_dot_yaml)
* static LandmarkBasePtr create(const YAML::Node& _lmk_yaml_node)
* static LandmarkBasePtr create(const YAML::Node& _lmk_yaml_node)
* \endcode
*
* See further down for an implementation example.
......@@ -137,7 +172,7 @@ namespace wolf
* that knows how to create your specific object, e.g.:
*
* \code
* FactoryLandmark::get().registerCreator("POLYLINE 2d", LandmarkPolyline2d::create);
* FactoryLandmark::get().registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
* \endcode
*
* #### Automatic registration
......@@ -145,7 +180,7 @@ namespace wolf
* For example, in sensor_camera_yaml.cpp we find the line:
*
* \code
* const bool registered_camera_intr = FactoryParamsSensor::get().registerCreator("CAMERA", createParamsSensorCamera);
* const bool registered_camera_intr = FactoryParamsSensor::get().registerCreator("ParamsSensorCamera", createParamsSensorCamera);
* \endcode
*
* which is a static invocation (i.e., it is placed at global scope outside of the ParamsSensorCamera class).
......@@ -156,7 +191,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("CAMERA");
* Factory<MyTypeBase, MyTypeInput>::get().unregisterCreator("ParamsSensorCamera");
* \endcode
*
* #### Create objects using the factory
......@@ -166,13 +201,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("POLYLINE 2d", lmk_yaml_node);
* LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::get().create("LandmarkPolyline2d", lmk_yaml_node);
* \endcode
*
* or even better, make use of the convenient typedefs:
*
* \code
* LandmarkBasePtr lmk_ptr = FactoryLandmark::get().create("POLYLINE 2d", lmk_yaml_node);
* LandmarkBasePtr lmk_ptr = FactoryLandmark::get().create("LandmarkPolyline2d", lmk_yaml_node);
* \endcode
*
* ### Examples
......@@ -197,7 +232,7 @@ namespace wolf
* }
*
* // Create a new landmark
* LandmarkBasePtr lmk_ptr = new LandmarkPolyline2d(points, first_defined, last_defined, first_id);
* LandmarkBasePtr lmk_ptr = std::make_shared<LandmarkPolyline2d>(points, first_defined, last_defined, first_id);
* lmk_ptr->setId(id);
*
* return lmk_ptr;
......@@ -212,7 +247,7 @@ namespace wolf
* // Register landmark creator (put the register code inside an unnamed namespace):
* namespace
* {
* const bool registered_lmk_polyline_2d = FactoryLandmark::get().registerCreator("POLYLINE 2d", LandmarkPolyline2d::create);
* const bool registered_lmk_polyline_2d = FactoryLandmark::get().registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
* }
*
* \endcode
......@@ -234,12 +269,16 @@ namespace wolf
template<class TypeBase, typename... TypeInput>
class Factory
{
private:
typedef std::shared_ptr<TypeBase> TypeBasePtr;
public:
// example of creator callback (see typedefs below)
typedef TypeBasePtr (*CreatorCallback)(TypeInput... _input);
private:
typedef std::map<std::string, CreatorCallback> CallbackMap;
public:
bool registerCreator(const std::string& _type, CreatorCallback createFn);
bool unregisterCreator(const std::string& _type);
......
......@@ -29,15 +29,6 @@
namespace wolf {
/**
* \brief scalar type for the Wolf project
*
* Do NEVER forget to use Wolf scalar definitions when you code!!!
*
* Do NEVER use plain Eigen scalar types!!! (This is redundant with the above. But just to make sure you behave!)
*/
//typedef double double;
/**
* \brief Vector and Matrices size type for the Wolf project
*
......
......@@ -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.