Skip to content
Snippets Groups Projects
Commit 0cc12df8 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

hotfix (lost commit)

parent d7e54ff8
No related branches found
No related tags found
2 merge requests!30Release after RAL,!29After 2nd RAL submission
...@@ -31,6 +31,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2> ...@@ -31,6 +31,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
bool _apply_loss_function, bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) : FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPoint2d,2,2,1,2,1,2>("FactorPoint2d", FactorAutodiff<FactorPoint2d,2,2,1,2,1,2>("FactorPoint2d",
_ftr_ptr,
nullptr, nullptr, nullptr, _lmk_ptr, nullptr, nullptr, nullptr, _lmk_ptr,
_processor_ptr, _processor_ptr,
_apply_loss_function, _apply_loss_function,
...@@ -46,16 +47,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2> ...@@ -46,16 +47,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
{ {
//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl; //std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
//std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl; //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
}
virtual ~FactorPoint2d() = default;
virtual void updateMeasurementAndSquareRootInformationUpper() override
{
if (not getFeature())
return;
// cast
auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature()); auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature());
// measurement // measurement
...@@ -68,6 +60,8 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2> ...@@ -68,6 +60,8 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance.inverse()); measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance.inverse());
} }
virtual ~FactorPoint2d() = default;
virtual std::string getTopology() const override virtual std::string getTopology() const override
{ {
return std::string("LMK"); return std::string("LMK");
......
...@@ -30,6 +30,7 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1, ...@@ -30,6 +30,7 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id, unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,2,2>("FactorPointToLine2d", FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,2,2>("FactorPointToLine2d",
_ftr_ptr,
nullptr, nullptr,
nullptr, nullptr,
nullptr, nullptr,
...@@ -52,14 +53,6 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1, ...@@ -52,14 +53,6 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
//std::cout << "FactorPointToLine2d" << std::endl; //std::cout << "FactorPointToLine2d" << std::endl;
//std::cout << "Landmark " << _lmk_ptr->id() << " first " << _lmk_ptr->getFirstId() << ", last " << _lmk_ptr->getLastId() << " isValid(ctr points):" << (_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_) ? "YES" : "NO") << std::endl; //std::cout << "Landmark " << _lmk_ptr->id() << " first " << _lmk_ptr->getFirstId() << ", last " << _lmk_ptr->getLastId() << " isValid(ctr points):" << (_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_) ? "YES" : "NO") << std::endl;
assert(_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_)); assert(_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_));
}
virtual ~FactorPointToLine2d() = default;
virtual void updateMeasurementAndSquareRootInformationUpper() override
{
if (not getFeature())
return;
// cast // cast
auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature()); auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature());
...@@ -74,6 +67,8 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1, ...@@ -74,6 +67,8 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse()); measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
} }
virtual ~FactorPointToLine2d() = default;
virtual std::string getTopology() const override virtual std::string getTopology() const override
{ {
return std::string("GEOM"); return std::string("GEOM");
......
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