Skip to content
Snippets Groups Projects
Commit 5f51324c authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Remove constructor from size...

It is required that the measurement and covariance are set at
construction time. This avoids dangling with null covariances which are
not invertible.
parent 01afec1a
No related branches found
No related tags found
No related merge requests found
......@@ -6,23 +6,6 @@ namespace wolf {
unsigned int FeatureBase::feature_id_count_ = 0;
FeatureBase::FeatureBase(const std::string& _type, unsigned int _dim_measurement) :
NodeBase("FEATURE", _type),
capture_ptr_(),
is_removing_(false),
feature_id_(++feature_id_count_),
track_id_(0),
landmark_id_(0),
measurement_(_dim_measurement),
measurement_covariance_(_dim_measurement, _dim_measurement),
measurement_sqrt_information_upper_(_dim_measurement,_dim_measurement)
{
measurement_covariance_.setZero();
measurement_sqrt_information_upper_.setIdentity();
measurement_sqrt_information_upper_ *= 1e8;
// std::cout << "constructed +f" << id() << std::endl;
}
FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
NodeBase("FEATURE", _type),
capture_ptr_(),
......
......@@ -37,11 +37,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
Eigen::VectorXs expectation_; ///< expectation
public:
/** \brief Constructor from capture pointer and measure dim
* \param _tp type of feature -- see wolf.h
* \param _dim_measurement the dimension of the measurement space
*/
FeatureBase(const std::string& _type, unsigned int _dim_measurement);
/** \brief Constructor from capture pointer and measure
* \param _tp type of feature -- see wolf.h
......
......@@ -2,12 +2,6 @@
namespace wolf {
FeatureGPSFix::FeatureGPSFix(unsigned int _dim_measurement) :
FeatureBase("GPS FIX", _dim_measurement)
{
//
}
FeatureGPSFix::FeatureGPSFix(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
FeatureBase("GPS FIX", _measurement, _meas_covariance)
{
......
......@@ -15,12 +15,6 @@ WOLF_PTR_TYPEDEFS(FeatureGPSFix);
class FeatureGPSFix : public FeatureBase
{
public:
/** \brief Constructor from capture pointer and measure dim
*
* \param _dim_measurement the dimension of the measurement space
*
*/
FeatureGPSFix(unsigned int _dim_measurement);
/** \brief Constructor from capture pointer and measure
*
......
......@@ -2,12 +2,6 @@
namespace wolf {
FeatureOdom2D::FeatureOdom2D(unsigned int _dim_measurement) :
FeatureBase("ODOM 2D", _dim_measurement)
{
//
}
FeatureOdom2D::FeatureOdom2D(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
FeatureBase("ODOM 2D", _measurement, _meas_covariance)
{
......
......@@ -17,12 +17,6 @@ WOLF_PTR_TYPEDEFS(FeatureOdom2D);
class FeatureOdom2D : public FeatureBase
{
public:
/** \brief Constructor from capture pointer and measure dim
*
* \param _dim_measurement the dimension of the measurement space
*
*/
FeatureOdom2D(unsigned int _dim_measurement);
/** \brief Constructor from capture pointer and measure
*
......
......@@ -33,12 +33,6 @@ class FeaturePolyline2D : public FeatureBase
int getNPoints() const;
};
inline FeaturePolyline2D::FeaturePolyline2D(const Eigen::MatrixXs& _points, const Eigen::MatrixXs& _points_cov, const bool& _first_defined, const bool& _last_defined) :
FeatureBase("POLYLINE 2D", 0), points_(_points), points_cov_(_points_cov), first_defined_(_first_defined), last_defined_(_last_defined)
{
assert(points_.rows() == 3 && points_cov_.rows() == 2 && points_cov_.cols() == 2*points_.cols() && "FeaturePolyline2D::FeaturePolyline2D: Bad points or covariance matrix size");
}
inline FeaturePolyline2D::~FeaturePolyline2D()
{
//
......
......@@ -13,18 +13,7 @@
using namespace wolf;
using namespace Eigen;
TEST(FeatureBase, ConstructorFromSize)
{
FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", 3));
ASSERT_EQ(f->getMeasurement().size() , 3);
ASSERT_EQ(f->getMeasurementCovariance().rows() , 3);
ASSERT_EQ(f->getMeasurementCovariance().cols() , 3);
ASSERT_EQ(f->getMeasurementSquareRootInformationUpper().rows() , 3);
ASSERT_EQ(f->getMeasurementSquareRootInformationUpper().cols() , 3);
}
TEST(FeatureBase, ConstructorFromMeasurementAndCovariance)
TEST(FeatureBase, Constructor)
{
Vector3s m; m << 1,2,3;
Matrix3s M; M.setRandom(); M = M*M.transpose();
......@@ -50,7 +39,7 @@ TEST(FeatureBase, SetMeasurement)
Eigen::MatrixXs U = llt_of_info.matrixU();
Eigen::MatrixXs L = llt_of_info.matrixL();
FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", 3));
FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3s::Zero(), Matrix3s::Identity()));
f->setMeasurement(m);
......@@ -66,7 +55,7 @@ TEST(FeatureBase, SetMeasurementCovariance)
Eigen::MatrixXs U = llt_of_info.matrixU();
Eigen::MatrixXs L = llt_of_info.matrixL();
FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", 3));
FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3s::Zero(), Matrix3s::Identity()));
f->setMeasurementCovariance(M);
......
......@@ -75,7 +75,7 @@ TEST(FrameBase, LinksToTree)
T->addFrame(F2);
CaptureMotionPtr C = make_shared<CaptureMotion>(1, S, Vector3s::Zero(), 3, 3);
F1->addCapture(C);
FeatureBasePtr f = make_shared<FeatureBase>("f", 1);
FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
C->addFeature(f);
ConstraintOdom2DPtr c = make_shared<ConstraintOdom2D>(f, F2);
f->addConstraint(c);
......
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