Merge branch '9-loss-function-api' into 'devel'

Resolve "Loss function API"

Closes #9

See merge request !16

include/IMU/factor/factor_IMU.h

@@ -19,8 +19,8 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
     public:
         FactorIMU(const FeatureIMUPtr& _ftr_ptr,
                       const CaptureIMUPtr& _capture_origin_ptr,
-                      const ProcessorBasePtr& _processor_ptr = nullptr,
-                      bool _apply_loss_function = false,
+                      const ProcessorBasePtr& _processor_ptr,
+                      bool _apply_loss_function,
                       FactorStatus _status = FAC_ACTIVE);
 
         virtual ~FactorIMU() = default;

include/IMU/factor/factor_fix_bias.h

@@ -19,7 +19,7 @@ WOLF_PTR_TYPEDEFS(FactorFixBias);
 class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3>
 {
     public:
-        FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+        FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
                 FactorAutodiff<FactorFixBias, 6, 3, 3>("FactorFixBias",
                         nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getAccBias(),
                                           std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getGyroBias())

src/feature/feature_apriltag.cpp

-
-#include "base/feature/feature_apriltag.h"
-
-namespace wolf {
-
-FeatureApriltag::FeatureApriltag(const Eigen::Vector7d & _measurement,
-                                 const Eigen::Matrix6d & _meas_uncertainty,
-                                 const int _tag_id,
-                                 const apriltag_detection_t & _det,
-                                 double _rep_error1,
-                                 double _rep_error2,
-                                 bool _use_rotation,
-                                 UncertaintyType _uncertainty_type) :
-    FeatureBase("FeatureApriltag", _measurement, _meas_uncertainty, _uncertainty_type),
-    tag_id_     (_tag_id),
-    tag_corners_(4),
-    detection_  (_det),
-    rep_error1_(_rep_error1),
-    rep_error2_(_rep_error2),
-    use_rotation_(_use_rotation)
-{
-    assert(_det.id == _tag_id && "Tag ID and detection ID do not match!");
-    setTrackId(_tag_id);
-    for (int i = 0; i < 4; i++)
-    {
-        tag_corners_[i].x = detection_.p[i][0];
-        tag_corners_[i].y = detection_.p[i][1];
-    }
-}
-
-FeatureApriltag::~FeatureApriltag()
-{
-    //
-}
-
-double FeatureApriltag::getTagId() const
-{
-    return tag_id_;
-}
-
-const apriltag_detection_t& FeatureApriltag::getDetection() const
-{
-    return detection_;
-}
-
-const std::vector<cv::Point2d>& FeatureApriltag::getTagCorners() const
-{
-    return tag_corners_;
-}
-
-double FeatureApriltag::getRepError1() const
-{
-    return rep_error1_;
-}
-
-double FeatureApriltag::getRepError2() const
-{
-    return rep_error2_;
-}
-
-bool FeatureApriltag::getUserotation() const
-{
-    return use_rotation_;
-}
-
-} // namespace wolf

src/landmark/landmark_apriltag.cpp

-
-#include "base/landmark/landmark_apriltag.h"
-#include "base/common/factory.h"
-#include "base/math/rotations.h"
-#include "base/yaml/yaml_conversion.h"
-
-namespace wolf {
-
-LandmarkApriltag::LandmarkApriltag(Eigen::Vector7d& pose, const int& _tagid, const double& _tag_width) :
-	LandmarkBase("LandmarkApriltag", std::make_shared<StateBlock>(pose.head(3)), std::make_shared<StateQuaternion>(pose.tail(4))),
-	tag_id_(_tagid),
-	tag_width_(_tag_width)
-{
-  	setDescriptor(Eigen::VectorXd::Constant(1,_tagid)); //change tagid to int ? do not use descriptor vector ?
-    setId(_tagid); // overwrite lmk ID to match tag_id.
-}
-
-LandmarkApriltag::~LandmarkApriltag()
-{
-    //
-}
-
-
-double LandmarkApriltag::getTagWidth() const
-{
-    return tag_width_;
-}
-
-int LandmarkApriltag::getTagId() const
-{
-    return tag_id_;
-}
-
-// LANDMARK SAVE AND LOAD FROM YAML
-
-// static
-LandmarkBasePtr LandmarkApriltag::create(const YAML::Node& _lmk_node)
-{
-    // Parse YAML node with lmk info and data
-    unsigned int    id                      = _lmk_node["id"]                   .as<unsigned int>();
-    unsigned int    tag_id                  = _lmk_node["tag id"]               .as<unsigned int>();
-    double          tag_width               = _lmk_node["tag width"]            .as<double>();
-    Eigen::Vector3d pos                     = _lmk_node["position"]             .as<Eigen::Vector3d>();
-    bool            pos_fixed               = _lmk_node["position fixed"]       .as<bool>();
-    Eigen::Vector4d vquat;
-    if (_lmk_node["orientation"].size() == 3)
-    {
-        // we have been given 3 Euler angles in degrees
-        Eigen::Vector3d   euler = M_TORAD * ( _lmk_node["orientation"]          .as<Eigen::Vector3d>() );
-        Eigen::Matrix3d       R = e2R(euler);
-        Eigen::Quaterniond quat = R2q(R);
-        vquat                   = quat.coeffs();
-    }
-    else if (_lmk_node["orientation"].size() == 4)
-    {
-        // we have been given a quaternion
-        vquat                               = _lmk_node["orientation"]          .as<Eigen::Vector4d>();
-    }
-    bool            ori_fixed               = _lmk_node["orientation fixed"]    .as<bool>();
-
-    Eigen::Vector7d pose; pose << pos, vquat;
-
-    // Create a new landmark
-    LandmarkApriltagPtr lmk_ptr = std::make_shared<LandmarkApriltag>(pose, tag_id, tag_width);
-    lmk_ptr->setId(id);
-    lmk_ptr->getP()->setFixed(pos_fixed);
-    lmk_ptr->getO()->setFixed(ori_fixed);
-
-    return lmk_ptr;
-
-}
-
-YAML::Node LandmarkApriltag::saveToYaml() const
-{
-    // First base things
-    YAML::Node node = LandmarkBase::saveToYaml();
-
-    // Then Apriltag specific things
-    node["tag id"] = getTagId();
-    node["tag width"] = getTagWidth();
-
-    return node;
-}
-
-
-// Register landmark creator
-namespace
-{
-const bool WOLF_UNUSED registered_lmk_apriltag = LandmarkFactory::get().registerCreator("LandmarkApriltag", LandmarkApriltag::create);
-}
-
-
-} // namespace wolf

src/processor/processor_IMU.cpp

@@ -94,7 +94,7 @@ FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, Captur
     CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
     FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
 
-    auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this());
+    auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
 
     return fac_imu;
 }

src/solver_suitesparse/solver_manager.cpp

-#include "core/ceres_wrapper/ceres_manager.h"
-
-SolverManager::SolverManager()
-{
-
-}
-
-SolverManager::~SolverManager()
-{
-	removeAllStateUnits();
-}
-
-void SolverManager::solve()
-{
-
-}
-
-//void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr)
-//{
-//}
-
-void SolverManager::update(const WolfProblemPtr _problem_ptr)
-{
-	// IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
-	if (_problem_ptr->isReallocated())
-	{
-	    // todo: reallocate x
-	}
-	else
-	{
-		// ADD/UPDATE STATE UNITS
-		for(auto state_unit_it = _problem_ptr->getStateList().begin(); state_unit_it!=_problem_ptr->getStateList().end(); state_unit_it++)
-		{
-			if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
-				addStateUnit(*state_unit_it);
-
-			else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
-				updateStateUnitStatus(*state_unit_it);
-		}
-		//std::cout << "state units updated!" << std::endl;
-
-		// REMOVE STATE UNITS
-		while (!_problem_ptr->getRemovedStateList().empty())
-		{
-			// TODO: remove state unit
-			//_problem_ptr->getRemovedStateList().pop_front();
-		}
-		//std::cout << "state units removed!" << std::endl;
-
-		// ADD CONSTRAINTS
-		FactorBasePtrList fac_list;
-		_problem_ptr->getTrajectory()->getFactorList(fac_list);
-		//std::cout << "fac_list.size() = " << fac_list.size() << std::endl;
-		for(auto fac_it = fac_list.begin(); fac_it!=fac_list.end(); fac_it++)
-			if ((*fac_it)->getPendingStatus() == ADD_PENDING)
-				addFactor(*fac_it);
-
-		//std::cout << "factors updated!" << std::endl;
-	}
-}
-
-void SolverManager::addFactor(FactorBasePtr _corr_ptr)
-{
-	//TODO MatrixXd J; Vector e;
-    // getResidualsAndJacobian(_corr_ptr, J, e);
-    // solverQR->addFactor(_corr_ptr, J, e);
-
-//	factor_map_[_corr_ptr->id()] = blockIdx;
-	_corr_ptr->setPendingStatus(NOT_PENDING);
-}
-
-void SolverManager::removeFactor(const unsigned int& _corr_idx)
-{
-    // TODO
-}
-
-void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
-{
-	//std::cout << "Adding State Unit " << _st_ptr->id() << std::endl;
-	//_st_ptr->print();
-
-	switch (_st_ptr->getStateType())
-	{
-		case ST_COMPLEX_ANGLE:
-		{
-		    // TODO
-			//std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
-			//ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
-			break;
-		}
-		case ST_THETA:
-		{
-			//std::cout << "No Local Parametrization to be added" << std::endl;
-			ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
-			break;
-		}
-		case ST_POINT_1D:
-		{
-			//std::cout << "No Local Parametrization to be added" << std::endl;
-			ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
-			break;
-		}
-		case ST_VECTOR:
-		{
-			//std::cout << "No Local Parametrization to be added" << std::endl;
-			ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
-			break;
-		}
-		case ST_POINT_3D:
-		{
-			//std::cout << "No Local Parametrization to be added" << std::endl;
-			ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
-			break;
-		}
-		default:
-			std::cout << "Unknown  Local Parametrization type!" << std::endl;
-	}
-	if (_st_ptr->isFixed())
-		updateStateUnitStatus(_st_ptr);
-
-	_st_ptr->setPendingStatus(NOT_PENDING);
-}
-
-void SolverManager::removeAllStateUnits()
-{
-	std::vector<double*> parameter_blocks;
-
-	ceres_problem_->GetParameterBlocks(&parameter_blocks);
-
-	for (unsigned int i = 0; i< parameter_blocks.size(); i++)
-		ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
-}
-
-void SolverManager::updateStateUnitStatus(StateBlockPtr _st_ptr)
-{
-    // TODO
-
-//	if (!_st_ptr->isFixed())
-//		ceres_problem_->SetParameterBlockVariable(_st_ptr->get());
-//	else if (_st_ptr->isFixed())
-//		ceres_problem_->SetParameterBlockConstant(_st_ptr->get());
-//	else
-//		printf("\nERROR: Update state unit status with unknown status");
-//
-//	_st_ptr->setPendingStatus(NOT_PENDING);
-}
-
-ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
-{
-	//std::cout << "adding ctr " << _corrPtr->id() << std::endl;
-	//_corrPtr->print();
-
-	switch (_corrPtr->getFactorType())
-	{
-		case FAC_GPS_FIX_2D:
-		{
-			FactorGPS2D* specific_ptr = (FactorGPS2D*)(_corrPtr);
-			return new ceres::AutoDiffCostFunction<FactorGPS2D,
-													specific_ptr->residualSize,
-													specific_ptr->block0Size,
-													specific_ptr->block1Size,
-													specific_ptr->block2Size,
-													specific_ptr->block3Size,
-													specific_ptr->block4Size,
-													specific_ptr->block5Size,
-													specific_ptr->block6Size,
-													specific_ptr->block7Size,
-													specific_ptr->block8Size,
-													specific_ptr->block9Size>(specific_ptr);
-			break;
-		}
-		case FAC_ODOM_2D_COMPLEX_ANGLE:
-		{
-			FactorOdom2DComplexAngle* specific_ptr = (FactorOdom2DComplexAngle*)(_corrPtr);
-			return new ceres::AutoDiffCostFunction<FactorOdom2DComplexAngle,
-													specific_ptr->residualSize,
-													specific_ptr->block0Size,
-													specific_ptr->block1Size,
-													specific_ptr->block2Size,
-													specific_ptr->block3Size,
-													specific_ptr->block4Size,
-													specific_ptr->block5Size,
-													specific_ptr->block6Size,
-													specific_ptr->block7Size,
-													specific_ptr->block8Size,
-													specific_ptr->block9Size>(specific_ptr);
-			break;
-		}
-		case FAC_ODOM_2D:
-		{
-			FactorOdom2D* specific_ptr = (FactorOdom2D*)(_corrPtr);
-			return new ceres::AutoDiffCostFunction<FactorOdom2D,
-													specific_ptr->residualSize,
-													specific_ptr->block0Size,
-													specific_ptr->block1Size,
-													specific_ptr->block2Size,
-													specific_ptr->block3Size,
-													specific_ptr->block4Size,
-													specific_ptr->block5Size,
-													specific_ptr->block6Size,
-													specific_ptr->block7Size,
-													specific_ptr->block8Size,
-													specific_ptr->block9Size>(specific_ptr);
-			break;
-		}
-		case FAC_CORNER_2D:
-		{
-			FactorCorner2D* specific_ptr = (FactorCorner2D*)(_corrPtr);
-			return new ceres::AutoDiffCostFunction<FactorCorner2D,
-													specific_ptr->residualSize,
-													specific_ptr->block0Size,
-													specific_ptr->block1Size,
-													specific_ptr->block2Size,
-													specific_ptr->block3Size,
-													specific_ptr->block4Size,
-													specific_ptr->block5Size,
-													specific_ptr->block6Size,
-													specific_ptr->block7Size,
-													specific_ptr->block8Size,
-													specific_ptr->block9Size>(specific_ptr);
-			break;
-		}
-		case FAC_IMU:
-		{
-			FactorIMU* specific_ptr = (FactorIMU*)(_corrPtr);
-			return new ceres::AutoDiffCostFunction<FactorIMU,
-													specific_ptr->residualSize,
-													specific_ptr->block0Size,
-													specific_ptr->block1Size,
-													specific_ptr->block2Size,
-													specific_ptr->block3Size,
-													specific_ptr->block4Size,
-													specific_ptr->block5Size,
-													specific_ptr->block6Size,
-													specific_ptr->block7Size,
-													specific_ptr->block8Size,
-													specific_ptr->block9Size>(specific_ptr);
-			break;
-		}
-		default:
-			std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
-
-			return nullptr;
-	}
-}