diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 04dc8922322eae1890cbbcb1769a99c5a4e2d6eb..88ff12b8d47ee72be9b164b23912dec3d94696ec 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -64,6 +64,8 @@ SET(HDRS sensor_gps_fix.h state_base.h state_point.h + state_orientation.h + state_theta.h state_complex_angle.h time_stamp.h trajectory_base.h @@ -91,6 +93,8 @@ SET(SRCS sensor_odom_2D.cpp sensor_gps_fix.cpp state_base.cpp + state_orientation.cpp + state_theta.cpp state_complex_angle.cpp time_stamp.cpp trajectory_base.cpp diff --git a/src/capture_gps_fix.cpp b/src/capture_gps_fix.cpp index 2217d43280abcfa479e481e77a4f8caecdbcb842..bd366c0332436c7c7ec2c40391e029fd9314abe7 100644 --- a/src/capture_gps_fix.cpp +++ b/src/capture_gps_fix.cpp @@ -26,11 +26,9 @@ CaptureGPSFix::~CaptureGPSFix() void CaptureGPSFix::processCapture() { // EXTRACT AND ADD FEATURES -// FeatureBase* new_feature = new FeatureGPSFix(data_,data_covariance_); addFeature(new FeatureGPSFix(data_,data_covariance_)); // ADD CONSTRAINT -// ConstraintBase* gps_constraint = new ConstraintGPS2D(getFeatureListPtr()->front(), getFramePtr()->getPPtr()); getFeatureListPtr()->front()->addConstraint(new ConstraintGPS2D(getFeatureListPtr()->front(), getFramePtr()->getPPtr())); } diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp index fd09248ade5284fc0ee24977c323ecb1e30fcd02..5a8e564b5be2f787ffdc4cd67e3185b291a32c13 100644 --- a/src/capture_laser_2D.cpp +++ b/src/capture_laser_2D.cpp @@ -32,29 +32,32 @@ CaptureLaser2D::~CaptureLaser2D() void CaptureLaser2D::processCapture() { //variables - std::list<Eigen::Vector4s> corners; + //std::list<Eigen::Vector4s> corners; + std::list<laserscanutils::Corner> corners; //extract corners from range data extractCorners(corners); //std::cout << corners.size() << " corners extracted" << std::endl; //generate a feature for each corner - //std::cout << "CaptureLaser2D::createFeatures..." << std::endl; createFeatures(corners); + //std::cout << getFeatureListPtr()->size() << " Features created" << std::endl; //Establish constraints for each feature - //std::cout << "CaptureLaser2D::establishConstraints..." << std::endl; establishConstraints(); + //std::cout << "Constraints created" << std::endl; } -unsigned int CaptureLaser2D::extractCorners(std::list<Eigen::Vector4s> & _corner_list) const +unsigned int CaptureLaser2D::extractCorners(std::list<laserscanutils::Corner> & _corner_list) const { return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, _corner_list); } -void CaptureLaser2D::createFeatures(std::list<Eigen::Vector4s> & _corner_list) +void CaptureLaser2D::createFeatures(std::list<laserscanutils::Corner> & _corner_list) { + // TODO: Sensor model Eigen::Matrix4s cov_mat; + Eigen::Vector4s meas; //init constant cov cov_mat << 0.01, 0, 0, 0, @@ -65,8 +68,11 @@ void CaptureLaser2D::createFeatures(std::list<Eigen::Vector4s> & _corner_list) //for each corner in the list create a feature for (auto corner_it = _corner_list.begin(); corner_it != _corner_list.end(); corner_it ++) { - (*corner_it)(2) = 0; - this->addFeature( (FeatureBase*)(new FeatureCorner2D( (*corner_it), cov_mat ) ) ); + meas.head(2) = (*corner_it).pt_.head(2); + meas(2) = (*corner_it).orientation_; + meas(3) = (*corner_it).aperture_; + //TODO: add the rest of descriptors and struct atributes + this->addFeature( (FeatureBase*)(new FeatureCorner2D( meas, cov_mat ) ) ); } } @@ -74,21 +80,14 @@ void CaptureLaser2D::establishConstraints() { // Global transformation TODO: Change by a function Eigen::Vector2s t_robot(*getFramePtr()->getPPtr()->getPtr(),*(getFramePtr()->getPPtr()->getPtr()+1)); - WolfScalar o = *(getFramePtr()->getOPtr()->getPtr()); - Eigen::Matrix2s R_robot; - if (getFramePtr()->getOPtr()->getStateType() == ST_THETA) - R_robot << cos(o),-sin(o), - sin(o), cos(o); - else - { - //TODO - } + Eigen::Matrix2s R_robot = ((StateOrientation*)(getFramePtr()->getOPtr()))->getRotationMatrix().topLeftCorner<2,2>(); + WolfScalar& robot_orientation = *(getFramePtr()->getOPtr()->getPtr()); // Sensor transformation Eigen::Vector2s t_sensor = getSensorPtr()->getSensorPosition()->head(2); - Eigen::Matrix2s R_sensor = getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>().transpose(); + Eigen::Matrix2s R_sensor = getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>(); - //Brute force closest (xy and theta) landmark search + //Brute force closest (xy and theta) landmark search //TODO: B&B // std::cout << "Brute force closest (xy and theta) landmark search: N features:" << getFeatureListPtr()->size() << std::endl; // std::cout << "N landmark:" << getTop()->getMapPtr()->getLandmarkListPtr()->size() << std::endl; // std::cout << "Vehicle transformation: " << std::endl; @@ -96,30 +95,37 @@ void CaptureLaser2D::establishConstraints() // std::cout << "rot:" << R << std::endl; for (auto feature_it = getFeatureListPtr()->begin(); feature_it != getFeatureListPtr()->end(); feature_it++ ) { - double max_distance_matching2 = 0.5; //TODO: max_distance_matching depending on localization and landmarks uncertainty - double max_theta_matching = M_PI / 8; //TODO: max_theta_matching depending on localization and landmarks uncertainty + WolfScalar max_distance_matching2 = 0.5; + WolfScalar max_theta_matching = M_PI / 8; //Find the closest landmark to the feature LandmarkCorner2D* correspondent_landmark = nullptr; - Eigen::Map<Eigen::Vector2s> feature_position((*feature_it)->getMeasurementPtr()->data()); - Eigen::Map<Eigen::Vector1s> feature_orientation = ((*feature_it)->getMeasurementPtr()->data()+2); + const Eigen::Vector2s& feature_position = (*feature_it)->getMeasurement().head(2); + const WolfScalar& feature_orientation = (*feature_it)->getMeasurement()(2); + const WolfScalar& feature_aperture = (*feature_it)->getMeasurement()(3); Eigen::Vector2s feature_global_position = R_robot * (R_sensor * feature_position + t_sensor) + t_robot; - Eigen::Vector1s feature_global_orientation; - feature_global_orientation(0) = feature_orientation(0) + o + atan2(R_sensor(1,0),R_sensor(0,0)); + WolfScalar feature_global_orientation = feature_orientation + robot_orientation + atan2(R_sensor(1,0),R_sensor(0,0)); + // fit in (-pi, pi] + feature_global_orientation = (feature_global_orientation > 0 ? fmod(feature_global_orientation+M_PI, 2 * M_PI)-M_PI : fmod(feature_global_orientation-M_PI, 2 * M_PI)+M_PI); //feature_global_orientation(0) = 0; double min_distance2 = max_distance_matching2; -// std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl; + std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl; // std::cout << "local position: " << feature_position.transpose() << " orientation:" << feature_orientation << std::endl; -// std::cout << "global position:" << feature_global_position.transpose() << " orientation:" << feature_global_orientation << std::endl; + std::cout << "global position: " << feature_global_position.transpose() << + " orientation: " << feature_global_orientation << + " aperture:" << feature_aperture << std::endl; for (auto landmark_it = getTop()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != getTop()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ ) { Eigen::Map<Eigen::Vector2s> landmark_position((*landmark_it)->getPPtr()->getPtr()); - WolfScalar landmark_orientation = *((*landmark_it)->getOPtr()->getPtr()); + std::cout << "landmark: " << (*landmark_it)->nodeId() << " " << landmark_position.transpose(); + WolfScalar& landmark_orientation = *((*landmark_it)->getOPtr()->getPtr()); + std::cout << " orientation: " << landmark_orientation << std::endl; + const WolfScalar& landmark_aperture = (*landmark_it)->getDescriptor()(0); WolfScalar distance2 = (landmark_position-feature_global_position).transpose() * (landmark_position-feature_global_position); - WolfScalar theta_distance = fabs(landmark_orientation-feature_global_orientation(0)); + WolfScalar theta_distance = fabs(landmark_orientation-feature_global_orientation); if (theta_distance > M_PI) theta_distance -= 2 * M_PI; @@ -127,40 +133,46 @@ void CaptureLaser2D::establishConstraints() { if (theta_distance < max_theta_matching) { -// std::cout << "Position & orientation near landmark found: " << (*landmark_it)->nodeId() << std::endl; -// std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl; + std::cout << "Position & orientation near landmark found: " << (*landmark_it)->nodeId() << std::endl; + std::cout << "global position: " << landmark_position.transpose() << " orientation: " << landmark_orientation << std::endl; correspondent_landmark = (LandmarkCorner2D*)(*landmark_it); min_distance2 = distance2; } else { - std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl; - std::cout << "global position:" << feature_global_position.transpose() << " orientation:" << feature_global_orientation << std::endl; - std::cout << "Landmark with near position but wrong orientation: " << (*landmark_it)->nodeId() << std::endl; - std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl; +// std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl; +// std::cout << "global position:" << feature_global_position.transpose() << +// " orientation:" << feature_global_orientation << +// " aperture:" << feature_aperture << std::endl; +// std::cout << "Landmark with near position but wrong orientation: " << (*landmark_it)->nodeId() << std::endl; +// std::cout << "global position:" << landmark_position.transpose() << +// " orientation:" << landmark_orientation << +// " aperture:" << landmark_aperture << std::endl; } } } if (correspondent_landmark == nullptr) { - //std::cout << "No landmark found. Creating a new one..." << std::endl; + std::cout << "No landmark found. Creating a new one..." << std::endl; StateBase* new_landmark_state_position = new StatePoint2D(getTop()->getNewStatePtr()); getTop()->addState(new_landmark_state_position, feature_global_position); StateBase* new_landmark_state_orientation = new StateTheta(getTop()->getNewStatePtr()); - getTop()->addState(new_landmark_state_orientation, feature_global_orientation); + getTop()->addState(new_landmark_state_orientation, Eigen::Map<Eigen::Vector1s>(&feature_global_orientation,1)); - correspondent_landmark = new LandmarkCorner2D(new_landmark_state_position, new_landmark_state_orientation); + correspondent_landmark = new LandmarkCorner2D(new_landmark_state_position, new_landmark_state_orientation, feature_aperture); LandmarkBase* corr_landmark(correspondent_landmark); getTop()->getMapPtr()->addLandmark(corr_landmark); - //std::cout << "Landmark created: " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << std::endl; - //std::cout << "global position: " << *corr_landmark->getPPtr()->getPtr() << " " << *(corr_landmark->getPPtr()->getPtr()+1) << " orientation:" << *corr_landmark->getOPtr()->getPtr() << std::endl; +// std::cout << "Landmark created: " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << std::endl; +// std::cout << "global position: " << *corr_landmark->getPPtr()->getPtr() << " " << *(corr_landmark->getPPtr()->getPtr()+1) << +// " orientation: " << *corr_landmark->getOPtr()->getPtr() << +// " aperture:" << corr_landmark->getDescriptor()(0) << std::endl; } else correspondent_landmark->hit(); - //std::cout << "Creating new constraint: Landmark " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << " & feature " << (*feature_it)->nodeId() << std::endl; +// std::cout << "Creating new constraint: Landmark " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << " & feature " << (*feature_it)->nodeId() << std::endl; // Add constraint to the correspondent landmark (*feature_it)->addConstraint(new ConstraintCorner2DTheta(*feature_it, diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h index 648c0bd404b63bdfd29c616b4824022f64c66af5..8f4c537af9b9f54dd98f60a46a723086a8d5fdde 100644 --- a/src/capture_laser_2D.h +++ b/src/capture_laser_2D.h @@ -10,8 +10,9 @@ #include <eigen3/Eigen/Geometry> //laser_scan_utils -#include "iri-algorithms/laser_scan_utils/scan_params.h" -#include "iri-algorithms/laser_scan_utils/corners.h" +#include "iri-algorithms/laser_scan_utils/scan_basics.h" +#include "iri-algorithms/laser_scan_utils/corner_detector.h" +#include "iri-algorithms/laser_scan_utils/entities.h" //wolf includes #include "constraint_corner_2D_theta.h" @@ -20,6 +21,8 @@ #include "feature_corner_2D.h" #include "landmark_corner_2D.h" #include "state_point.h" +#include "state_orientation.h" +#include "state_theta.h" //wolf forward declarations //#include "feature_corner_2D.h" @@ -73,7 +76,7 @@ class CaptureLaser2D : public CaptureBase * Extract corners and push-back to Feature down list . * **/ - virtual unsigned int extractCorners(std::list<Eigen::Vector4s> & _corner_list) const; + virtual unsigned int extractCorners(std::list<laserscanutils::Corner> & _corner_list) const; // virtual unsigned int extractCorners_old(std::list<Eigen::Vector4s> & _corner_list) const; // void fitLine(unsigned int _idx_from, unsigned int _idx_to, const Eigen::MatrixXs& _points, Line& line_) const; @@ -86,7 +89,7 @@ class CaptureLaser2D : public CaptureBase // void fitLine(unsigned int _idx_from, unsigned int _idx_to, const Eigen::MatrixXs& _points, Line& line_) const; - virtual void createFeatures(std::list<Eigen::Vector4s> & _corner_list); //TODO: should be const .... JVN: No, because new feature is added to the list + virtual void createFeatures(std::list<laserscanutils::Corner> & _corner_list); //TODO: should be const .... JVN: No, because new feature is added to the list void establishConstraints(); diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 5948c5e8a8f6160814ea0049352c80175cae34ba..4d34b7c5fbe159d2d5019e128f30e9bb1040d060 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -8,17 +8,10 @@ CeresManager::CeresManager(ceres::Problem::Options _options) : covariance_options.null_space_rank = -1; covariance_ = new ceres::Covariance(covariance_options); } -//num_threads(1), use_dense_linear_algebra(true), min_singular_value_threshold(1e-12), null_space_rank(-1), apply_loss_function(false) - CeresManager::~CeresManager() { - std::vector<double*> state_units; - - ceres_problem_->GetParameterBlocks(&state_units); - - for (uint i = 0; i< state_units.size(); i++) - removeStateUnit(state_units.at(i)); + removeAllStateUnits(); std::cout << "all state units removed! \n"; std::cout << "residual blocks: " << ceres_problem_->NumResidualBlocks() << "\n"; @@ -42,24 +35,8 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_ return ceres_summary_; } -void CeresManager::computeCovariances(WolfProblem* _problem_ptr)//StateBaseList* _state_list_ptr, StateBase* _current_state_unit) +void CeresManager::computeCovariances(WolfProblem* _problem_ptr) { -// std::cout << "_state_list_ptr.size() " << _state_list_ptr->size() << std::endl; -// -// // create vector of pointer pairs -// std::vector<std::pair<const double*, const double*>> covariance_blocks; -//// for (auto st_it = _state_list_ptr->begin(); st_it != _state_list_ptr->end(); st_it++) -//// if ((*st_it)->getPtr() != _current_state_unit->getPtr()) -//// covariance_blocks.push_back(std::pair<const double*, const double*>((*st_it)->getPtr(),_current_state_unit->getPtr())); -// -// WolfScalar* block_1_ptr = _current_state_unit->getPtr(); -// WolfScalar* block_2_ptr = _current_state_unit->getPtr(); -// std::cout << "are blocks? " << ceres_problem_->HasParameterBlock(block_1_ptr) << ceres_problem_->HasParameterBlock(block_2_ptr) << std::endl; -// covariance_blocks.push_back(std::make_pair(block_1_ptr,block_2_ptr)); -// std::cout << "covariance_blocks.size() " << covariance_blocks.size() << std::endl; -// // Compute covariances -// covariance_.Compute(covariance_blocks, ceres_problem_); - std::vector<std::pair<const double*, const double*>> covariance_blocks; // Last frame state units @@ -99,63 +76,69 @@ void CeresManager::computeCovariances(WolfProblem* _problem_ptr)//StateBaseList* void CeresManager::update(WolfProblem* _problem_ptr) { - // ADD/UPDATE STATE UNITS - for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++) + // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN + if (_problem_ptr->isReallocated()) { - if ((*state_unit_it)->getPendingStatus() == ADD_PENDING) + // Remove all parameter blocks (residual blocks will be also removed) + removeAllStateUnits(); + + // Add all parameter blocks + for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++) addStateUnit(*state_unit_it); - else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING) - updateStateUnitStatus(*state_unit_it); + // Add all residual blocks + ConstraintBaseList ctr_list; + _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list); + for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++) + addConstraint(*ctr_it); } - //std::cout << "state units updated!" << std::endl; - - // REMOVE STATE UNITS - while (!_problem_ptr->getRemovedStateListPtr()->empty()) + else { - removeStateUnit(_problem_ptr->getRemovedStateListPtr()->front()); - _problem_ptr->getRemovedStateListPtr()->pop_front(); - } - //std::cout << "state units removed!" << std::endl; + // ADD/UPDATE STATE UNITS + for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++) + { + if ((*state_unit_it)->getPendingStatus() == ADD_PENDING) + addStateUnit(*state_unit_it); - // ADD CONSTRAINTS - ConstraintBaseList ctr_list; - _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list); - //std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl; - for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++) - if ((*ctr_it)->getPendingStatus() == ADD_PENDING) - addConstraint((*ctr_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->getRemovedStateListPtr()->empty()) + { + ceres_problem_->RemoveParameterBlock(_problem_ptr->getRemovedStateListPtr()->front()); + _problem_ptr->getRemovedStateListPtr()->pop_front(); + } + //std::cout << "state units removed!" << std::endl; - //std::cout << "constraints updated!" << std::endl; + // ADD CONSTRAINTS + ConstraintBaseList ctr_list; + _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list); + //std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl; + for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++) + if ((*ctr_it)->getPendingStatus() == ADD_PENDING) + addConstraint(*ctr_it); + + //std::cout << "constraints updated!" << std::endl; + } } void CeresManager::addConstraint(ConstraintBase* _corr_ptr) { ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getStateBlockPtrVector()); // constraint_map_[_corr_ptr->nodeId()] = blockIdx; - _corr_ptr->setPendingStatus(NOT_PENDING); } -void CeresManager::addConstraints(ConstraintBaseList* _new_constraints_list_ptr) -{ - //std::cout << _new_constraints.size() << " new constraints\n"; - for(auto constraint_it = _new_constraints_list_ptr->begin(); constraint_it!=_new_constraints_list_ptr->end(); constraint_it++) - addConstraint(*constraint_it); -} - void CeresManager::removeConstraint(const unsigned int& _corr_idx) { + // TODO: necessari? outliers? // ceres_problem_->RemoveResidualBlock(constraint_map_[_corr_idx]); // constraint_map_.erase(_corr_idx); } -void CeresManager::removeConstraints(const std::list<unsigned int>& _corr_idx_list) -{ - for (auto idx_it=_corr_idx_list.begin(); idx_it!=_corr_idx_list.end(); idx_it++) - removeConstraint(*idx_it); -} - void CeresManager::addStateUnit(StateBase* _st_ptr) { //std::cout << "Adding State Unit " << _st_ptr->nodeId() << std::endl; @@ -176,7 +159,13 @@ void CeresManager::addStateUnit(StateBase* _st_ptr) // ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateQuaternion*)_st_ptr.get())->BLOCK_SIZE, new QuaternionParameterization); // break; // } - case ST_POINT_1D: // equivalent ST_THETA: + case ST_THETA: + { + //std::cout << "No Local Parametrization to be added" << std::endl; + ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateTheta*)_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->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr); @@ -203,21 +192,14 @@ void CeresManager::addStateUnit(StateBase* _st_ptr) _st_ptr->setPendingStatus(NOT_PENDING); } -void CeresManager::addStateUnits(StateBaseList* _new_state_units) +void CeresManager::removeAllStateUnits() { - for(auto state_unit_it = _new_state_units->begin(); state_unit_it!=_new_state_units->end(); state_unit_it++) - addStateUnit(*state_unit_it); -} + std::vector<double*> parameter_blocks; -void CeresManager::removeStateUnit(WolfScalar* _st_ptr) -{ - ceres_problem_->RemoveParameterBlock(_st_ptr); -} + ceres_problem_->GetParameterBlocks(¶meter_blocks); -void CeresManager::removeStateUnits(std::list<WolfScalar*> _st_ptr_list) -{ - for(auto state_unit_it = _st_ptr_list.begin(); state_unit_it!=_st_ptr_list.end(); state_unit_it++) - removeStateUnit(*state_unit_it); + for (unsigned int i = 0; i< parameter_blocks.size(); i++) + ceres_problem_->RemoveParameterBlock(parameter_blocks[i]); } void CeresManager::updateStateUnitStatus(StateBase* _st_ptr) @@ -232,12 +214,6 @@ void CeresManager::updateStateUnitStatus(StateBase* _st_ptr) _st_ptr->setPendingStatus(NOT_PENDING); } -void CeresManager::updateStateUnitStatus(StateBaseList* _st_ptr_list) -{ - for(auto state_unit_it = _st_ptr_list->begin(); state_unit_it!=_st_ptr_list->end(); state_unit_it++) - updateStateUnitStatus(*state_unit_it); -} - ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr) { //std::cout << "adding ctr " << _corrPtr->nodeId() << std::endl; diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h index 5b19540f531f5748c58406a22dc034fd892c3d98..74b81d5eac3ac20f2fd7191c8e6ddc6d09584386 100644 --- a/src/ceres_wrapper/ceres_manager.h +++ b/src/ceres_wrapper/ceres_manager.h @@ -11,6 +11,7 @@ #include "state_base.h" #include "state_point.h" #include "state_complex_angle.h" +#include "state_theta.h" #include "constraint_sparse.h" #include "constraint_gps_2D.h" #include "constraint_odom_2D_theta.h" @@ -38,35 +39,21 @@ class CeresManager ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options); - void computeCovariances(WolfProblem* _problem_ptr);//StateBaseList* _state_units_list, StateBase* _current_state_unit); + void computeCovariances(WolfProblem* _problem_ptr); void update(const WolfProblemPtr _problem_ptr); void addConstraint(ConstraintBase* _corr_ptr); - // TODO: not necessary - void addConstraints(ConstraintBaseList* _new_constraints_list_ptr); - - // TODO: not necessary + // TODO: not necessary? void removeConstraint(const unsigned int& _corr_idx); - // TODO: not necessary - void removeConstraints(const std::list<unsigned int>& _corr_idx_list); - void addStateUnit(StateBase* _st_ptr); - // TODO: not necessary - void addStateUnits(StateBaseList* _st_ptr_list); - - void removeStateUnit(WolfScalar* _st_ptr); - - void removeStateUnits(std::list<WolfScalar*> _st_ptr_list); + void removeAllStateUnits(); void updateStateUnitStatus(StateBase* _st_ptr); - // TODO: not necessary - void updateStateUnitStatus(StateBaseList* _st_ptr_list); - ceres::CostFunction* createCostFunction(ConstraintBase* _corrPtr); }; diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp index d493e76e96cb62f2ef1efda87bbffebb67db036e..6d1f1c614e2557f0567cd9d4c4938faabbd87c64 100644 --- a/src/constraint_base.cpp +++ b/src/constraint_base.cpp @@ -3,8 +3,8 @@ ConstraintBase::ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp) : NodeLinked(BOTTOM, "CONSTRAINT"), type_(_tp), - measurement_ptr_(_ftr_ptr->getMeasurementPtr()), - measurement_covariance_ptr_(_ftr_ptr->getMeasurementCovariancePtr()) + measurement_(_ftr_ptr->getMeasurement()), + measurement_covariance_(_ftr_ptr->getMeasurementCovariance()) { // } @@ -19,9 +19,9 @@ ConstraintType ConstraintBase::getConstraintType() const return type_; } -const Eigen::VectorXs * ConstraintBase::getMeasurementPtr() +const Eigen::VectorXs& ConstraintBase::getMeasurement() { - return upperNodePtr()->getMeasurementPtr(); + return measurement_; } FeatureBase* ConstraintBase::getFeaturePtr() const diff --git a/src/constraint_base.h b/src/constraint_base.h index c167cbaa64c651653631ad53e8cdf758731e576f..4824c899658284f3c3bd1698cc4b217cd164eafb 100644 --- a/src/constraint_base.h +++ b/src/constraint_base.h @@ -21,8 +21,8 @@ class ConstraintBase : public NodeLinked<FeatureBase,NodeTerminus> { protected: ConstraintType type_; //type of constraint (types defined at wolf.h) - Eigen::VectorXs * measurement_ptr_; // TODO:TBD: pointer, map or copy of the feature measurement? - Eigen::MatrixXs * measurement_covariance_ptr_; // TODO:TBD: pointer, map or copy of the feature measurement covariance? + const Eigen::VectorXs& measurement_; // TODO:TBD: pointer, map or copy of the feature measurement? + const Eigen::MatrixXs& measurement_covariance_; // TODO:TBD: pointer, map or copy of the feature measurement covariance? public: /** \brief Constructor @@ -58,7 +58,7 @@ class ConstraintBase : public NodeLinked<FeatureBase,NodeTerminus> * Returns a pointer to the feature measurement * **/ - const Eigen::VectorXs * getMeasurementPtr(); + const Eigen::VectorXs& getMeasurement(); /** \brief Returns a pointer to its capture * diff --git a/src/constraint_corner_2D_theta.h b/src/constraint_corner_2D_theta.h index 60d4a5fbe9f3fb0a16f174884e126464a135628c..06a3a370c907a8db9759d88f3c9841f7a1279ba3 100644 --- a/src/constraint_corner_2D_theta.h +++ b/src/constraint_corner_2D_theta.h @@ -56,7 +56,6 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1> // std::cout << getCapturePtr()->getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>() << std::endl; // std::cout << "atan2: " << atan2(getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,1),getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,0)) << std::endl; - // sensor transformation Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getSensorPosition()->head(2).cast<T>(); Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>().transpose()).cast<T>(); @@ -75,9 +74,9 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1> expected_landmark_relative_orientation = expected_landmark_relative_orientation + T(2*M_PI); // Residuals - _residuals[0] = (expected_landmark_relative_position(0) - T((*measurement_ptr_)(0))) / T((*measurement_covariance_ptr_)(0,0)); - _residuals[1] = (expected_landmark_relative_position(1) - T((*measurement_ptr_)(1))) / T((*measurement_covariance_ptr_)(1,1)); - _residuals[2] = (expected_landmark_relative_orientation - T((*measurement_ptr_)(2))) / T(100*(*measurement_covariance_ptr_)(2,2)); + _residuals[0] = (expected_landmark_relative_position(0) - T(measurement_(0))) / T(measurement_covariance_(0,0)); + _residuals[1] = (expected_landmark_relative_position(1) - T(measurement_(1))) / T(measurement_covariance_(1,1)); + _residuals[2] = (expected_landmark_relative_orientation - T(measurement_(2))) / T(100*measurement_covariance_(2,2)); // std::cout << "\nCONSTRAINT: " << nodeId() << std::endl; // std::cout << "Feature: " << getFeaturePtr()->nodeId() << std::endl; diff --git a/src/constraint_gps_2D.h b/src/constraint_gps_2D.h index 8d1648b2bcb30c4782db2281cb31694d21cfd44b..f3e82559f3e265cc53510e255cef8c1d832ee200 100644 --- a/src/constraint_gps_2D.h +++ b/src/constraint_gps_2D.h @@ -31,8 +31,8 @@ class ConstraintGPS2D: public ConstraintSparse<2,2> template <typename T> bool operator()(const T* const _x, T* _residuals) const { - _residuals[0] = (T((*measurement_ptr_)(0)) - _x[0]) / T((*measurement_covariance_ptr_)(0,0)); - _residuals[1] = (T((*measurement_ptr_)(1)) - _x[1]) / T((*measurement_covariance_ptr_)(1,1)); + _residuals[0] = (T(measurement_(0)) - _x[0]) / T(measurement_covariance_(0,0)); + _residuals[1] = (T(measurement_(1)) - _x[1]) / T(measurement_covariance_(1,1)); return true; } }; diff --git a/src/constraint_odom_2D_complex_angle.h b/src/constraint_odom_2D_complex_angle.h index 965393ba7e7b3fff4eb97b69066ac2848e8aa9f7..320e86f0ac4f54db8f1b4ddf76a9f4e5baf6da9b 100644 --- a/src/constraint_odom_2D_complex_angle.h +++ b/src/constraint_odom_2D_complex_angle.h @@ -36,8 +36,8 @@ class ConstraintOdom2DComplexAngle: public ConstraintSparse<2,2,2,2,2> T expected_rotation = atan2(_o2[1]*_o1[0] - _o2[0]*_o1[1], _o1[0]*_o2[0] + _o1[1]*_o2[1]); // Residuals - _residuals[0] = (expected_range - T((*measurement_ptr_)(0))*T((*measurement_ptr_)(0))) / T((*measurement_covariance_ptr_)(0,0)); - _residuals[1] = (expected_rotation - T((*measurement_ptr_)(1))) / T((*measurement_covariance_ptr_)(1,1)); + _residuals[0] = (expected_range - T(measurement_(0))*T(measurement_(0))) / T(measurement_covariance_(0,0)); + _residuals[1] = (expected_rotation - T(measurement_(1))) / T(measurement_covariance_(1,1)); return true; } diff --git a/src/constraint_odom_2D_theta.h b/src/constraint_odom_2D_theta.h index 97b55fe1c7041f9f6f193e934669b1941c093d37..f637169c3156d499f82f147c7f1f3259d21bbad8 100644 --- a/src/constraint_odom_2D_theta.h +++ b/src/constraint_odom_2D_theta.h @@ -36,8 +36,8 @@ class ConstraintOdom2DTheta: public ConstraintSparse<2,2,1,2,1> T expected_rotation = _o2[0]-_o1[0]; // Residuals - _residuals[0] = (expected_range - T((*measurement_ptr_)(0))*T((*measurement_ptr_)(0))) / T((*measurement_covariance_ptr_)(0,0)); - _residuals[1] = (expected_rotation - T((*measurement_ptr_)(1))) / T((*measurement_covariance_ptr_)(1,1)); + _residuals[0] = (expected_range - T(measurement_(0))*T(measurement_(0))) / T(measurement_covariance_(0,0)); + _residuals[1] = (expected_rotation - T(measurement_(1))) / T(measurement_covariance_(1,1)); return true; } diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 1d9713765430ad1a450822f4d8ae13f272b46e32..849bbf0537273a806af510f5c3d4b6b7f548280b 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -35,8 +35,8 @@ TARGET_LINK_LIBRARIES(test_ceres_odom_iterative ${PROJECT_NAME}) # TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME}) # test ceres manager, wolf manager and wolf tree -ADD_EXECUTABLE(test_ceres_manager_tree test_ceres_manager_tree.cpp) -TARGET_LINK_LIBRARIES(test_ceres_manager_tree ${PROJECT_NAME}) +#ADD_EXECUTABLE(test_ceres_manager_tree test_ceres_manager_tree.cpp) +#TARGET_LINK_LIBRARIES(test_ceres_manager_tree ${PROJECT_NAME}) # test ceres covariance ADD_EXECUTABLE(test_ceres_covariance test_ceres_covariance.cpp) diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index 31bc5d1003c034d99e171a1f03f5d5705572ae4b..874d524f2fae4d097656f6901d442cf7d60f2842 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -50,6 +50,10 @@ #include "faramotics/rangeScan2D.h" #include "btr-headers/pose3d.h" +//laser_scan_utils +#include "iri-algorithms/laser_scan_utils/corner_detector.h" +#include "iri-algorithms/laser_scan_utils/entities.h" + //function travel around void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_) { @@ -123,7 +127,6 @@ class WolfManager { createFrame(_init_frame, TimeStamp()); problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix(); - } virtual ~WolfManager() @@ -172,22 +175,8 @@ class WolfManager // ODOMETRY SENSOR if (new_capture->getSensorPtr() == sensor_prior_) { - // FIND PREVIOUS RELATIVE CAPTURE -// CaptureRelative* previous_relative_capture = nullptr; -// for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++) -// { -// //std::cout << "capture: " << (*capture_it)->nodeId() << std::endl; -// if ((*capture_it)->getSensorPtr() == sensor_prior_) -// { -// previous_relative_capture = (CaptureRelative*)(*capture_it); -// break; -// } -// } - // INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR //std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl; -// previous_relative_capture->integrateCapture((CaptureRelative*)(new_capture)); -// Eigen::VectorXs prior = previous_relative_capture->computePrior(); last_capture_relative_->integrateCapture((CaptureRelative*)(new_capture)); // NEW KEY FRAME (if enough time from last frame) @@ -434,14 +423,14 @@ int main(int argc, char** argv) // DRAWING STUFF --------------------------- t1=clock(); // draw detected corners - std::list<Eigen::Vector4s> corner_list; + std::list<laserscanutils::Corner> corner_list; std::vector<double> corner_vector; CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1_reading); last_scan.extractCorners(corner_list); - for (std::list<Eigen::Vector4s>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ ) + for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ ) { - corner_vector.push_back(corner_it->x()); - corner_vector.push_back(corner_it->y()); + corner_vector.push_back(corner_it->pt_(0)); + corner_vector.push_back(corner_it->pt_(1)); } myRender->drawCorners(laser1Pose,corner_vector); diff --git a/src/examples/test_ceres_laser.cpp b/src/examples/test_ceres_laser.cpp index 93bec962d253f6a166c58876c513160a675c873e..49dce88ab0c9c7c3e7598750e004553bc70f41be 100644 --- a/src/examples/test_ceres_laser.cpp +++ b/src/examples/test_ceres_laser.cpp @@ -50,6 +50,10 @@ #include "faramotics/rangeScan2D.h" #include "btr-headers/pose3d.h" +//laser_scan_utils +#include "iri-algorithms/laser_scan_utils/corner_detector.h" +#include "iri-algorithms/laser_scan_utils/entities.h" + //function travel around void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_) { @@ -436,14 +440,14 @@ int main(int argc, char** argv) // DRAWING STUFF --------------------------- t1=clock(); // draw detected corners - std::list<Eigen::Vector4s> corner_list; + std::list<laserscanutils::Corner> corner_list; std::vector<double> corner_vector; CaptureLaser2D last_scan(time_stamp, &laser_sensor, scan_reading); last_scan.extractCorners(corner_list); - for (std::list<Eigen::Vector4s>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ ) + for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ ) { - corner_vector.push_back(corner_it->x()); - corner_vector.push_back(corner_it->y()); + corner_vector.push_back(corner_it->pt_(0)); + corner_vector.push_back(corner_it->pt_(1)); } myRender->drawCorners(devicePose,corner_vector); diff --git a/src/examples/test_ceres_manager.cpp b/src/examples/test_ceres_manager.cpp index 9ad87ed7267cb2bab2e46764859f3626990479b4..591267237367312e9d23517d989b0e8b373e1f03 100644 --- a/src/examples/test_ceres_manager.cpp +++ b/src/examples/test_ceres_manager.cpp @@ -22,6 +22,7 @@ #include "frame_base.h" #include "state_point.h" #include "state_complex_angle.h" +#include "state_theta.h" #include "capture_base.h" #include "state_base.h" #include "wolf.h" diff --git a/src/feature_base.cpp b/src/feature_base.cpp index dca55969fb15fa1540b36849734b213cbc44b55b..fadc2ffaca6cba68a9a0fc804e8e59f05a705a97 100644 --- a/src/feature_base.cpp +++ b/src/feature_base.cpp @@ -46,14 +46,24 @@ void FeatureBase::getConstraintList(ConstraintBaseList & _ctr_list) _ctr_list.push_back((*c_it)); } -Eigen::VectorXs * FeatureBase::getMeasurementPtr() +//Eigen::VectorXs * FeatureBase::getMeasurementPtr() +//{ +// return &measurement_; +//} +// +//Eigen::MatrixXs * FeatureBase::getMeasurementCovariancePtr() +//{ +// return &measurement_covariance_; +//} + +const Eigen::VectorXs& FeatureBase::getMeasurement() const { - return &measurement_; + return measurement_; } -Eigen::MatrixXs * FeatureBase::getMeasurementCovariancePtr() +const Eigen::MatrixXs& FeatureBase::getMeasurementCovariance() const { - return &measurement_covariance_; + return measurement_covariance_; } void FeatureBase::setMeasurement(const Eigen::VectorXs & _meas) diff --git a/src/feature_base.h b/src/feature_base.h index fb0cd8ed03d397813ab4e7a68eeb6546e01cb5ae..59ffe6e31f026af54f550da703e63ef5ecb59a1a 100644 --- a/src/feature_base.h +++ b/src/feature_base.h @@ -48,9 +48,13 @@ class FeatureBase : public NodeLinked<CaptureBase,ConstraintBase> void getConstraintList(ConstraintBaseList & _ctr_list); - Eigen::VectorXs * getMeasurementPtr(); - - Eigen::MatrixXs * getMeasurementCovariancePtr(); +// Eigen::VectorXs * getMeasurementPtr(); +// +// Eigen::MatrixXs * getMeasurementCovariancePtr(); + + const Eigen::VectorXs & getMeasurement() const; + + const Eigen::MatrixXs & getMeasurementCovariance() const; void setMeasurement(const Eigen::VectorXs & _meas); diff --git a/src/feature_corner_2D.cpp b/src/feature_corner_2D.cpp index 38bec15120a5807060b76b3d715dfee2901e39ed..af9b5f16f2cfce004ea0e60f9a0d6d30c648488e 100644 --- a/src/feature_corner_2D.cpp +++ b/src/feature_corner_2D.cpp @@ -11,8 +11,3 @@ FeatureCorner2D::~FeatureCorner2D() { // } - -//void FeatureCorner2D::findConstraints() -//{ -// // -//} diff --git a/src/feature_corner_2D.h b/src/feature_corner_2D.h index d38fd57b615602895d0c4b78e6cf340397ccdf9e..770216a7a46e2ef28a9f032a25365f49c0653202 100644 --- a/src/feature_corner_2D.h +++ b/src/feature_corner_2D.h @@ -23,14 +23,6 @@ class FeatureCorner2D : public FeatureBase * */ virtual ~FeatureCorner2D(); - - //TODO: Encara que sigui brutforce jo la búsqueda la faria a nivell capture. - /** \brief Generic interface to find constraints - * - * Generic interface to find constraints between this feature and a map (static/slam) or a previous feature - * - **/ - //virtual void findConstraints(); }; #endif diff --git a/src/feature_gps_fix.cpp b/src/feature_gps_fix.cpp index 94ffd56f23d5bdbbce4c61d040d4d38b5997f07b..3106e34aab0682de69cb0de4de16ea97b7114fd3 100644 --- a/src/feature_gps_fix.cpp +++ b/src/feature_gps_fix.cpp @@ -16,9 +16,3 @@ FeatureGPSFix::~FeatureGPSFix() { // } - -void FeatureGPSFix::findConstraints() -{ - //ConstraintBase* gps_constraint(new ConstraintGPS2D(this, getCapturePtr()->getFramePtr()->getPPtr())); - addConstraint(new ConstraintGPS2D(this, getCapturePtr()->getFramePtr()->getPPtr())); -} diff --git a/src/feature_gps_fix.h b/src/feature_gps_fix.h index 3a0445afa44352a68de9587d1dbc4d7a709e00e3..9e7228548470e2977f4750d87fee38e788abfc52 100644 --- a/src/feature_gps_fix.h +++ b/src/feature_gps_fix.h @@ -27,13 +27,5 @@ class FeatureGPSFix : public FeatureBase FeatureGPSFix(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance); virtual ~FeatureGPSFix(); - - /** \brief Generic interface to find constraints - * - * Generic interface to find constraints between this feature and a map (static/slam) or a previous feature - * - **/ - virtual void findConstraints(); - }; #endif diff --git a/src/frame_base.h b/src/frame_base.h index 60d80b9d410ccd50a094eaf4eb7a3861e506e008..c52a5d9e6ad008870859c11b9d0a146cff44d7e8 100644 --- a/src/frame_base.h +++ b/src/frame_base.h @@ -27,7 +27,6 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase> FrameType type_; //type of frame. Either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h) TimeStamp time_stamp_; //frame time stamp StateStatus status_; // status of the estimation of the frame state - //Eigen::Vector3s state_; //TBD: Instead , It could be a vector/list/map of pointers to state units StateBase* p_ptr_; // Position state unit pointer StateBase* o_ptr_; // Orientation state unit pointer StateBase* v_ptr_; // Velocity state unit pointer @@ -116,6 +115,8 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase> StateBase* getWPtr() const; + const Eigen::Matrix4s * getTransformationMatrix() const; + virtual void printSelf(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const; }; diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp index 2c89acdb4e4879067ccd2c0fe461033c1d617264..dd9b2076a77f8a8b08863ad2e9010bcf71b2329f 100644 --- a/src/landmark_base.cpp +++ b/src/landmark_base.cpp @@ -116,7 +116,7 @@ void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor) descriptor_ = _descriptor; } -const Eigen::VectorXs LandmarkBase::getDescriptor() const +const Eigen::VectorXs& LandmarkBase::getDescriptor() const { return descriptor_; } diff --git a/src/landmark_base.h b/src/landmark_base.h index c5723e88e61b130c214231e6b9264249785eddf4..ea4cfe7da9e0ea6eea45fd2bff90a4659dbd31b8 100644 --- a/src/landmark_base.h +++ b/src/landmark_base.h @@ -85,7 +85,7 @@ class LandmarkBase : public NodeLinked<MapBase,NodeTerminus> void setDescriptor(const Eigen::VectorXs& _descriptor); - const Eigen::VectorXs getDescriptor() const; + const Eigen::VectorXs& getDescriptor() const; //const StateBase* getStatePtr() const; diff --git a/src/landmark_corner_2D.cpp b/src/landmark_corner_2D.cpp index 28d6a2b6249a4dfea0159e6066bbbe6db53dcb88..838d96159f5ff6605ddfac0c86b64e0a1e9480f4 100644 --- a/src/landmark_corner_2D.cpp +++ b/src/landmark_corner_2D.cpp @@ -4,8 +4,7 @@ LandmarkCorner2D::LandmarkCorner2D(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _aperture) : LandmarkBase(LANDMARK_CORNER, _p_ptr, _o_ptr) { - if (_aperture!=0) - setDescriptor(Eigen::Vector1s(_aperture)); + setDescriptor(Eigen::VectorXs::Constant(1,_aperture)); } LandmarkCorner2D::~LandmarkCorner2D() diff --git a/src/sensor_base.h b/src/sensor_base.h index c822a405c21c3f9907d0773bccb2760b55c508a5..0bab63e4115f67b010ce5bb78609bd448c474762 100644 --- a/src/sensor_base.h +++ b/src/sensor_base.h @@ -7,14 +7,19 @@ //Wolf includes #include "wolf.h" #include "node_base.h" +#include "state_point.h" +#include "state_orientation.h" class SensorBase : public NodeBase { protected: - SensorType type_;//indicates sensor type. Enum defined at wolf.h + SensorType type_; //indicates sensor type. Enum defined at wolf.h Eigen::Vector3s sensor_position_vehicle_;//sensor position in the vehicle frame Eigen::Matrix3s sensor_rotation_vehicle_;//sensor rotation in the vehicle frame - Eigen::VectorXs params_;//sensor intrinsic params: offsets, scale factors, sizes, ... + // TODO: +// StateBase* p_ptr_; // sensor position state unit pointer +// StateOrientation* o_ptr_; // sensor orientation state unit pointer + Eigen::VectorXs params_; //sensor intrinsic params: offsets, scale factors, sizes, ... //bool generate_prior_; //flag indicating if this sensor generates the prior or not public: diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h index c8fb24ce7d7596d730159bab756d663f0cbd43f5..1ffc79c37ed3a24986cc11ef8ce32b09f24d9065 100644 --- a/src/sensor_laser_2D.h +++ b/src/sensor_laser_2D.h @@ -4,8 +4,8 @@ #define SENSOR_LASER_2D_H_ //laser_scan_utils -#include "iri-algorithms/laser_scan_utils/scan_params.h" -#include "iri-algorithms/laser_scan_utils/corners.h" +#include "iri-algorithms/laser_scan_utils/scan_basics.h" +#include "iri-algorithms/laser_scan_utils/corner_detector.h" //wolf #include "sensor_base.h" diff --git a/src/state_complex_angle.cpp b/src/state_complex_angle.cpp index 8583434a7ab33ca46706c2acdda3553c707aa2b0..05a8853c7aa1815dc7304a3344d88311dff7881f 100644 --- a/src/state_complex_angle.cpp +++ b/src/state_complex_angle.cpp @@ -2,14 +2,13 @@ #include "state_complex_angle.h" StateComplexAngle::StateComplexAngle(Eigen::VectorXs& _st_remote, const unsigned int _idx) : - StateBase(_st_remote, _idx) + StateOrientation(_st_remote, _idx) { // - std::cout << "state complex angle created\n"; } StateComplexAngle::StateComplexAngle(WolfScalar* _st_ptr) : - StateBase(_st_ptr) + StateOrientation(_st_ptr) { // } @@ -29,10 +28,21 @@ unsigned int StateComplexAngle::getStateSize() const return BLOCK_SIZE; } +Eigen::Matrix3s StateComplexAngle::getRotationMatrix() const +{ + Eigen::Matrix3s R(Eigen::Matrix3s::Identity()); + R(0,0) = *state_ptr_; + R(1,1) = *state_ptr_; + R(0,1) = -*(state_ptr_+1); + R(1,0) = *(state_ptr_+1); + + return R; +} + void StateComplexAngle::print(unsigned int _ntabs, std::ostream& _ost) const { printTabs(_ntabs); _ost << nodeLabel() << " " << nodeId() << std::endl; printTabs(_ntabs); - _ost << *this->state_ptr_<< " " << *(this->state_ptr_+1) << std::endl; + _ost << *state_ptr_<< " " << *(state_ptr_+1) << std::endl; } diff --git a/src/state_complex_angle.h b/src/state_complex_angle.h index beef095022a45b8091b5acc84dc3ef6577538a7d..d4aab33c5b0b7fae84c26990077cded9367a581f 100644 --- a/src/state_complex_angle.h +++ b/src/state_complex_angle.h @@ -9,10 +9,10 @@ //Wolf includes #include "wolf.h" -#include "state_base.h" +#include "state_orientation.h" //class StateComplexAngle -class StateComplexAngle : public StateBase +class StateComplexAngle : public StateOrientation { public: static const unsigned int BLOCK_SIZE = 2; @@ -55,6 +55,13 @@ class StateComplexAngle : public StateBase **/ virtual unsigned int getStateSize() const; + /** \brief Returns the 3x3 rotation matrix of the orientation + * + * Returns the 3x3 rotation matrix of the orientation + * + **/ + virtual Eigen::Matrix3s getRotationMatrix() const; + /** \brief Prints all the elements of the state unit * * Prints all the elements of the state unit diff --git a/src/state_orientation.cpp b/src/state_orientation.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e48be883c7ec96077515bbb7bb46c886e160a9ba --- /dev/null +++ b/src/state_orientation.cpp @@ -0,0 +1,20 @@ + +#include "state_orientation.h" + +StateOrientation::StateOrientation(Eigen::VectorXs& _st_remote, const unsigned int _idx) : + StateBase(_st_remote, _idx) +{ + // +} + + +StateOrientation::StateOrientation(WolfScalar* _st_ptr) : + StateBase(_st_ptr) +{ + // +} + +StateOrientation::~StateOrientation() +{ + // +} diff --git a/src/state_orientation.h b/src/state_orientation.h new file mode 100644 index 0000000000000000000000000000000000000000..df5ee64272dbabd8b71f5d6ccb6a1b4cf1209599 --- /dev/null +++ b/src/state_orientation.h @@ -0,0 +1,50 @@ + +#ifndef STATE_ORIENTATION_H_ +#define STATE_ORIENTATION_H_ + +//std includes +#include <iostream> +#include <vector> +#include <cmath> + +//Wolf includes +#include "wolf.h" +#include "state_base.h" + +//class StateOrientation +class StateOrientation : public StateBase +{ + public: + + /** \brief Constructor with whole state storage and index where starts the state unit + * + * Constructor with whole state storage and index where starts the state unit + * \param _st_remote is the whole state vector + * \param _idx is the index of the first element of the state in the whole state vector + * + **/ + StateOrientation(Eigen::VectorXs& _st_remote, const unsigned int _idx); + + /** \brief Constructor with scalar pointer + * + * Constructor with scalar pointer + * \param _st_ptr is the pointer of the first element of the state unit + * + **/ + StateOrientation(WolfScalar* _st_ptr); + + /** \brief Destructor + * + * Destructor + * + **/ + virtual ~StateOrientation(); + + /** \brief Returns the 3x3 rotation matrix of the orientation + * + * Returns the 3x3 rotation matrix of the orientation + * + **/ + virtual Eigen::Matrix3s getRotationMatrix() const = 0; +}; +#endif diff --git a/src/state_point.h b/src/state_point.h index 0dfdc52280a1b8adb4f2b1fcc37d78da307b6966..7f515cc3f187ca3719b4365abd2a3c116ce46f4f 100644 --- a/src/state_point.h +++ b/src/state_point.h @@ -9,6 +9,7 @@ //Wolf includes #include "wolf.h" +#include "state_base.h" //class StateBase template <unsigned int SIZE> @@ -80,6 +81,16 @@ class StatePoint : public StateBase return BLOCK_SIZE; } + /** \brief Returns the point in a vector + * + * Returns the point in a vector + * + **/ + const Eigen::Matrix<WolfScalar, BLOCK_SIZE, 1>& getVector() const + { + return Eigen::Map<const Eigen::Matrix<WolfScalar, BLOCK_SIZE, 1>>(state_ptr_); + } + /** \brief Prints all the elements of the state unit * * Prints all the elements of the state unit diff --git a/src/state_theta.cpp b/src/state_theta.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b1e707b3e54f26d2b74a1c1bc756d20f3b411fc3 --- /dev/null +++ b/src/state_theta.cpp @@ -0,0 +1,50 @@ + +#include "state_theta.h" + +StateTheta::StateTheta(Eigen::VectorXs& _st_remote, const unsigned int _idx) : + StateOrientation(_st_remote, _idx) +{ + // +} + +StateTheta::StateTheta(WolfScalar* _st_ptr) : + StateOrientation(_st_ptr) +{ + // +} + +StateTheta::~StateTheta() +{ + // +} + +StateType StateTheta::getStateType() const +{ + return ST_THETA; +} + +unsigned int StateTheta::getStateSize() const +{ + return BLOCK_SIZE; +} + +Eigen::Matrix3s StateTheta::getRotationMatrix() const +{ + Eigen::Matrix3s R(Eigen::Matrix3s::Identity()); + R(0,0) = cos(*state_ptr_); + R(1,1) = cos(*state_ptr_); + R(0,1) = -sin(*state_ptr_); + R(1,0) = sin(*state_ptr_); + + return R; +} + +void StateTheta::print(unsigned int _ntabs, std::ostream& _ost) const +{ + printTabs(_ntabs); + _ost << nodeLabel() << " " << nodeId() << std::endl; + printTabs(_ntabs); + for (uint i = 0; i < BLOCK_SIZE; i++) + _ost << *(this->state_ptr_+i) << " "; + _ost << std::endl; +} diff --git a/src/state_theta.h b/src/state_theta.h new file mode 100644 index 0000000000000000000000000000000000000000..6c76502f2aec635eda799f4473662949e128eaee --- /dev/null +++ b/src/state_theta.h @@ -0,0 +1,72 @@ + +#ifndef STATE_THETA_H_ +#define STATE_THETA_H_ + +//std includes +#include <iostream> +#include <vector> +#include <cmath> + +//Wolf includes +#include "wolf.h" +#include "state_orientation.h" + +//class StateTheta +class StateTheta : public StateOrientation +{ + public: + static const unsigned int BLOCK_SIZE = 1; + + /** \brief Constructor with whole state storage and index where starts the state unit + * + * Constructor with whole state storage and index where starts the state unit + * \param _st_remote is the whole state vector + * \param _idx is the index of the first element of the state in the whole state vector + * + **/ + StateTheta(Eigen::VectorXs& _st_remote, const unsigned int _idx); + + /** \brief Constructor with scalar pointer + * + * Constructor with scalar pointer + * \param _st_ptr is the pointer of the first element of the state unit + * + **/ + StateTheta(WolfScalar* _st_ptr); + + /** \brief Destructor + * + * Destructor + * + **/ + virtual ~StateTheta(); + + /** \brief Returns the parametrization of the state unit + * + * Returns the parametrizationType (see wolf.h) of the state unit + * + **/ + virtual StateType getStateType() const; + + /** \brief Returns the state unit size + * + * Returns the parametrizationType (see wolf.h) of the state unit + * + **/ + virtual unsigned int getStateSize() const; + + /** \brief Returns the 3x3 rotation matrix of the orientation + * + * Returns the 3x3 rotation matrix of the orientation + * + **/ + virtual Eigen::Matrix3s getRotationMatrix() const; + + /** \brief Prints all the elements of the state unit + * + * Prints all the elements of the state unit + * + **/ + virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const; +}; +#endif diff --git a/src/wolf.h b/src/wolf.h index aee57e5d64369b8463533d66cfcef8b78f1eba1c..bcef332a035a3ecd2d965dc44cba6c08ff21ef51 100644 --- a/src/wolf.h +++ b/src/wolf.h @@ -126,12 +126,12 @@ typedef enum */ typedef enum { - ST_POINT_1D=1, ///< A 1D point. No parametrization. - ST_POINT_2D=2, ///< A 2D point. No parametrization. - ST_POINT_3D=3, ///< A 3D point. No parametrization. - ST_THETA=1, ///< A 2D orientation represented by a single angle. No parametrization (equivalent to ST_POINT_1D). - ST_COMPLEX_ANGLE=4, ///< A 2D orientation represented by a complex number. - ST_QUATERNION=5 ///< A 3D orientation represented by a quaternion. + ST_POINT_1D, ///< A 1D point. No parametrization. + ST_POINT_2D, ///< A 2D point. No parametrization. + ST_POINT_3D, ///< A 3D point. No parametrization. + ST_THETA, ///< A 2D orientation represented by a single angle. No parametrization. + ST_COMPLEX_ANGLE, ///< A 2D orientation represented by a complex number. + ST_QUATERNION ///< A 3D orientation represented by a quaternion. } StateType; /** \brief Enumeration of all possible state status @@ -298,7 +298,6 @@ typedef TransSensorMap::iterator TransSensorIter; typedef std::list<StateBase*> StateBaseList; typedef StateBaseList::iterator StateBaseIter; -typedef StatePoint<1> StateTheta; typedef StatePoint<1> StatePoint1D; typedef StatePoint<2> StatePoint2D; typedef StatePoint<3> StatePoint3D;