diff --git a/src/capture_imu.cpp b/src/capture_imu.cpp index 66cfc4d50ce5c9140da58639e4526ce237b97303..166841416a5098452d7914505c758a898633e1fb 100644 --- a/src/capture_imu.cpp +++ b/src/capture_imu.cpp @@ -11,7 +11,7 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, const TimeStamp& _final_ts, Se } CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, - const Eigen::Vector6s& _data, const Eigen::Matrix<WolfScalar,6,3>& _data_covariance) : + const Eigen::Vector6s& _data, const Eigen::Matrix<Scalar,6,3>& _data_covariance) : CaptureMotion(_init_ts, _final_ts, _sensor_ptr, _data, _data_covariance) { // diff --git a/src/capture_imu.h b/src/capture_imu.h index 1e35f2ed734c724c8bdf72ea860189d351c6cdd5..7e0dccaa6c738bf0fe6ab205e0956820cb6e9372 100644 --- a/src/capture_imu.h +++ b/src/capture_imu.h @@ -14,7 +14,7 @@ class CaptureIMU : public CaptureMotion CaptureIMU(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::Vector6s& _data); - CaptureIMU(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::Vector6s& _data, const Eigen::Matrix<WolfScalar,6,3>& _data_covariance); + CaptureIMU(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::Vector6s& _data, const Eigen::Matrix<Scalar,6,3>& _data_covariance); /** \brief Default destructor (not recommended) * diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp index fc86a3264d1d7805b5d13e4846d1e932b07a7c1e..a111ecbd60538240e319188ee223a1c305419380 100644 --- a/src/capture_odom_2D.cpp +++ b/src/capture_odom_2D.cpp @@ -98,7 +98,7 @@ void CaptureOdom2D::integrateCapture(CaptureMotion* _new_capture) CaptureOdom2D* CaptureOdom2D::interpolateCapture(const TimeStamp& _ts) { - WolfScalar ratio = (_ts.get() - this->time_stamp_.get()) / (this->final_time_stamp_.get() - this->time_stamp_.get()); + Scalar ratio = (_ts.get() - this->time_stamp_.get()) / (this->final_time_stamp_.get() - this->time_stamp_.get()); // Second part CaptureOdom2D* second_odom_ptr = new CaptureOdom2D(_ts, final_time_stamp_, sensor_ptr_, data_ * (1-ratio), data_covariance_ * (1-ratio)); diff --git a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h b/src/ceres_wrapper/auto_diff_cost_function_wrapper.h index a1e7e71c1cdb11821a61512462a9cae59fa7befa..151b77747df5308e1b96664bdfd95bee9ea9a4da 100644 --- a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h +++ b/src/ceres_wrapper/auto_diff_cost_function_wrapper.h @@ -17,7 +17,7 @@ class AutoDiffCostFunctionWrapperBase : public ceres::SizedCostFunction<MEASUREM BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE, BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE> { - typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + + typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + BLOCK_5_SIZE + BLOCK_6_SIZE + BLOCK_7_SIZE + BLOCK_8_SIZE + BLOCK_9_SIZE> WolfJet; protected: @@ -164,7 +164,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE, BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE, BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,0> { - typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + + typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + BLOCK_5_SIZE + BLOCK_6_SIZE + BLOCK_7_SIZE + BLOCK_8_SIZE> WolfJet; protected: @@ -302,7 +302,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE, BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE, BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,0,0> { - typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + + typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + BLOCK_5_SIZE + BLOCK_6_SIZE + BLOCK_7_SIZE> WolfJet; protected: @@ -433,7 +433,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE, BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE, BLOCK_5_SIZE,BLOCK_6_SIZE,0,0,0> { - typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + + typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + BLOCK_5_SIZE + BLOCK_6_SIZE> WolfJet; protected: @@ -557,7 +557,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE, BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE, BLOCK_5_SIZE,0,0,0,0> { - typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + + typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE + BLOCK_5_SIZE> WolfJet; protected: @@ -672,7 +672,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE, BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE, 0,0,0,0,0> { - typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE> WolfJet; + typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + BLOCK_4_SIZE> WolfJet; protected: ConstraintType* constraint_ptr_; @@ -778,7 +778,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE, BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,0, 0,0,0,0,0> { - typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE> WolfJet; + typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE> WolfJet; protected: ConstraintType* constraint_ptr_; @@ -874,7 +874,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE, BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,0,0, 0,0,0,0,0> { - typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE> WolfJet; + typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE> WolfJet; protected: ConstraintType* constraint_ptr_; @@ -965,7 +965,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE, BLOCK_0_SIZE,BLOCK_1_SIZE,0,0,0, 0,0,0,0,0> { - typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + BLOCK_1_SIZE> WolfJet; + typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE> WolfJet; protected: ConstraintType* constraint_ptr_; @@ -1048,7 +1048,7 @@ class AutoDiffCostFunctionWrapperBase<ConstraintType, MEASUREMENT_SIZE, BLOCK_0_SIZE,0,0,0,0, 0,0,0,0,0> { - typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE> WolfJet; + typedef ceres::Jet<Scalar, BLOCK_0_SIZE> WolfJet; protected: ConstraintType* constraint_ptr_; diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 9ff7999f5af5bc0fcf068b8637491398e9bf87a9..271130cf9f679918c5673edd7e3d5464df304a4d 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -155,7 +155,7 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) // STORE DESIRED COVARIANCES for (unsigned int i = 0; i < double_pairs.size(); i++) { - Eigen::Matrix<WolfScalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize()); + Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize()); covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data()); //std::cout << "getted covariance " << std::endl << cov << std::endl; wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov); diff --git a/src/constraint_analytic.cpp b/src/constraint_analytic.cpp index 148ac34d7ef15291eb547e3165fd8411ff6057ce..726af57ea4775158125bf9897a025df3d4d91298 100644 --- a/src/constraint_analytic.cpp +++ b/src/constraint_analytic.cpp @@ -51,7 +51,7 @@ ConstraintAnalytic::~ConstraintAnalytic() } -const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector() +const std::vector<Scalar*> ConstraintAnalytic::getStateBlockPtrVector() { assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10"); @@ -59,29 +59,29 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector() { case 1: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr()}); + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr()}); } case 2: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr()}); } case 3: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr()}); } case 4: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr()}); } case 5: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -89,7 +89,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector() } case 6: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -98,7 +98,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector() } case 7: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -108,7 +108,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector() } case 8: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -119,7 +119,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector() } case 9: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -131,7 +131,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector() } case 10: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -144,7 +144,7 @@ const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector() } } - return std::vector<WolfScalar*>(0); //Not going to happen + return std::vector<Scalar*>(0); //Not going to happen } const std::vector<StateBlock*> ConstraintAnalytic::getStatePtrVector() const diff --git a/src/constraint_analytic.h b/src/constraint_analytic.h index 1620d6e66de6253a1ad44020c3fb624e4c39df8b..ee87df853fdeeb823f3659c0f8add8d291156c08 100644 --- a/src/constraint_analytic.h +++ b/src/constraint_analytic.h @@ -96,7 +96,7 @@ class ConstraintAnalytic: public ConstraintBase * Returns a vector of pointers to the state blocks in which this constraint depends * **/ - virtual const std::vector<WolfScalar*> getStateBlockPtrVector(); + virtual const std::vector<Scalar*> getStateBlockPtrVector(); /** \brief Returns a vector of pointers to the states * diff --git a/src/constraint_base.h b/src/constraint_base.h index 735830ea27140fe8890fe04ac3156d74e3d75870..8dc4b96ee081b0a9d205154ff22af0d0d9b301e7 100644 --- a/src/constraint_base.h +++ b/src/constraint_base.h @@ -62,7 +62,7 @@ class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus> /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint **/ - virtual const std::vector<WolfScalar*> getStateBlockPtrVector() = 0; + virtual const std::vector<Scalar*> getStateBlockPtrVector() = 0; /** \brief Returns a vector of pointers to the states in which this constraint depends **/ diff --git a/src/constraint_container.h b/src/constraint_container.h index c92162372baa8351d1f59573e1b2027043b69d6f..b590e75d0452781ec62c559523579670c4eaa78b 100644 --- a/src/constraint_container.h +++ b/src/constraint_container.h @@ -59,7 +59,7 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1> // sensor transformation Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getVector().head(2).cast<T>(); // Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getRotationMatrix2D().transpose()).cast<T>(); - Eigen::Rotation2D<WolfScalar> S_R( getCapturePtr()->getSensorOPtr()->getVector()(0) ); + Eigen::Rotation2D<Scalar> S_R( getCapturePtr()->getSensorOPtr()->getVector()(0) ); Eigen::Matrix<T,2,2> inverse_R_sensor = (S_R.matrix().transpose()).cast<T>(); diff --git a/src/constraint_corner_2D.h b/src/constraint_corner_2D.h index e73d6dab2219792716f35c9f16f9975fc0fe9547..7c5f7c3af4acf76e77022a3c329ad429a3c6ec06 100644 --- a/src/constraint_corner_2D.h +++ b/src/constraint_corner_2D.h @@ -65,7 +65,7 @@ inline bool ConstraintCorner2D::operator ()(const T* const _robotP, const T* con // sensor transformation Eigen::Matrix<T, 2, 1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getVector().head(2).cast<T>(); // Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getRotationMatrix2D().transpose()).cast<T>(); - Eigen::Rotation2D<WolfScalar> S_R(getCapturePtr()->getSensorOPtr()->getVector()(0)); + Eigen::Rotation2D<Scalar> S_R(getCapturePtr()->getSensorOPtr()->getVector()(0)); Eigen::Matrix<T, 2, 2> inverse_R_sensor = (S_R.matrix().transpose()).cast<T>(); // robot information Eigen::Matrix<T, 2, 2> inverse_R_robot; diff --git a/src/constraint_epipolar.h b/src/constraint_epipolar.h index 1cb1e4416f1ff51cedf87b175def972343c77809..67834f596d913eebeacfd29d98ce8a83546f632e 100644 --- a/src/constraint_epipolar.h +++ b/src/constraint_epipolar.h @@ -18,7 +18,7 @@ class ConstraintEpipolar : public ConstraintBase /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint **/ - virtual const std::vector<WolfScalar*> getStateBlockPtrVector(){return std::vector<WolfScalar*>(0);} + virtual const std::vector<Scalar*> getStateBlockPtrVector(){return std::vector<Scalar*>(0);} /** \brief Returns a vector of pointers to the states in which this constraint depends **/ diff --git a/src/constraint_gps_pseudorange_2D.h b/src/constraint_gps_pseudorange_2D.h index 4c28b6e998f0167f4666c771317ca687e01662e5..9c07bb8aa6292a35b803eaaaf823b28e1895e794 100644 --- a/src/constraint_gps_pseudorange_2D.h +++ b/src/constraint_gps_pseudorange_2D.h @@ -72,7 +72,7 @@ public: protected: Eigen::Vector3s sat_position_; - WolfScalar pseudorange_; + Scalar pseudorange_; }; diff --git a/src/constraint_gps_pseudorange_3D.h b/src/constraint_gps_pseudorange_3D.h index 5e7b22ad8b26b6d12fc4b3e2cc0e5efa4e453e76..7d3e6a0b203b832186d9cbbb72dc42a7b63803ce 100644 --- a/src/constraint_gps_pseudorange_3D.h +++ b/src/constraint_gps_pseudorange_3D.h @@ -71,7 +71,7 @@ public: protected: Eigen::Vector3s sat_position_; - WolfScalar pseudorange_; + Scalar pseudorange_; }; diff --git a/src/constraint_odom_2D_analytic.h b/src/constraint_odom_2D_analytic.h index b45ee9c824c8c6486de5de47efad1d177a6fbe9b..7591b269372b3f77fd72679858f2b6ba03718713 100644 --- a/src/constraint_odom_2D_analytic.h +++ b/src/constraint_odom_2D_analytic.h @@ -47,7 +47,7 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic // Eigen::VectorXs expected_measurement(3); // // // Expected measurement -// Eigen::Matrix2s R = Eigen::Rotation2D<WolfScalar>(-_st_vector[1](0)).matrix(); +// Eigen::Matrix2s R = Eigen::Rotation2D<Scalar>(-_st_vector[1](0)).matrix(); // expected_measurement.head(2) = R * (_st_vector[2]-_st_vector[0]); // rotar menys l'angle de primer (-_o1) // expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0); // diff --git a/src/constraint_relative_2D_analytic.h b/src/constraint_relative_2D_analytic.h index 1f51f61a2132c3de3d3b1156b71568bbd6f9a03d..92a81a2d3c918645461dc217cb2ff5950a53df21 100644 --- a/src/constraint_relative_2D_analytic.h +++ b/src/constraint_relative_2D_analytic.h @@ -117,7 +117,7 @@ inline Eigen::VectorXs ConstraintRelative2DAnalytic::evaluateResiduals( Eigen::VectorXs residual(3); Eigen::VectorXs expected_measurement(3); // Expected measurement - Eigen::Matrix2s R = Eigen::Rotation2D<WolfScalar>(-_st_vector[1](0)).matrix(); + Eigen::Matrix2s R = Eigen::Rotation2D<Scalar>(-_st_vector[1](0)).matrix(); expected_measurement.head(2) = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1) expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0); // Residual diff --git a/src/constraint_sparse.h b/src/constraint_sparse.h index 497927ff23bc3951c77bc9503d94ae85cafa23cd..02e9339c6f1bc1dd118ddfba33ede097cac8c52f 100644 --- a/src/constraint_sparse.h +++ b/src/constraint_sparse.h @@ -121,7 +121,7 @@ class ConstraintSparse: public ConstraintBase * Returns a vector of pointers to the state blocks in which this constraint depends * **/ - virtual const std::vector<WolfScalar*> getStateBlockPtrVector(); + virtual const std::vector<Scalar*> getStateBlockPtrVector(); /** \brief Returns a vector of pointers to the states * @@ -338,7 +338,7 @@ template <const unsigned int MEASUREMENT_SIZE, unsigned int BLOCK_7_SIZE, unsigned int BLOCK_8_SIZE, unsigned int BLOCK_9_SIZE> -const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE, +const std::vector<Scalar*> ConstraintSparse<MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, @@ -356,29 +356,29 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE, { case 1: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr()}); + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr()}); } case 2: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr()}); } case 3: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr()}); } case 4: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr()}); } case 5: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -386,7 +386,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE, } case 6: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -395,7 +395,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE, } case 7: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -405,7 +405,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE, } case 8: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -416,7 +416,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE, } case 9: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -428,7 +428,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE, } case 10: { - return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(), + return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(), state_ptr_vector_[1]->getPtr(), state_ptr_vector_[2]->getPtr(), state_ptr_vector_[3]->getPtr(), @@ -441,7 +441,7 @@ const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE, } } - return std::vector<WolfScalar*>(0); //Not going to happen + return std::vector<Scalar*>(0); //Not going to happen } template <const unsigned int MEASUREMENT_SIZE, diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp index 9ee5dc67617d24ac39baf2d79b31754df277496f..173229b1b03a2016b0f888c3be0790536a077d35 100644 --- a/src/examples/solver/test_iQR_wolf2.cpp +++ b/src/examples/solver/test_iQR_wolf2.cpp @@ -120,10 +120,10 @@ int main(int argc, char *argv[]) //init random generators - WolfScalar odom_std_factor = 0.1; - WolfScalar gps_std = 10; + Scalar odom_std_factor = 0.1; + Scalar gps_std = 10; std::default_random_engine generator(1); - std::normal_distribution<WolfScalar> gaussian_distribution(0.0, 1); + std::normal_distribution<Scalar> gaussian_distribution(0.0, 1); // Faramotics stuff Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose; @@ -300,7 +300,7 @@ int main(int argc, char *argv[]) std::vector<double> landmark_vector; for (auto landmark_it = wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++) { - WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); + Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); landmark_vector.push_back(*position_ptr); //x landmark_vector.push_back(*(position_ptr + 1)); //y landmark_vector.push_back(0.2); //z @@ -355,7 +355,7 @@ int main(int argc, char *argv[]) std::vector<double> landmark_vector; for (auto landmark_it = wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++) { - WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); + Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); landmark_vector.push_back(*position_ptr); //x landmark_vector.push_back(*(position_ptr + 1)); //y landmark_vector.push_back(0.2); //z diff --git a/src/examples/test_analytic_odom_constraint.cpp b/src/examples/test_analytic_odom_constraint.cpp index fcc7a71b40e1bf172b3d3a719b9ef7b39f88d90c..f945d8c62333969d3de4a7ddace4be7c33852675 100644 --- a/src/examples/test_analytic_odom_constraint.cpp +++ b/src/examples/test_analytic_odom_constraint.cpp @@ -20,10 +20,10 @@ namespace wolf { // inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers -void insertSparseBlock(const Eigen::SparseMatrix<WolfScalar>& ins, Eigen::SparseMatrix<WolfScalar>& original, const unsigned int& row, const unsigned int& col) +void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col) { for (int k=0; k<ins.outerSize(); ++k) - for (Eigen::SparseMatrix<WolfScalar>::InnerIterator iti(ins,k); iti; ++iti) + for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti) original.coeffRef(iti.row() + row, iti.col() + col) = iti.value(); original.makeCompressed(); diff --git a/src/examples/test_autodiff.cpp b/src/examples/test_autodiff.cpp index aae3d005ee426444d856f7beb76e9dd4b3a87cef..e7dd1649541216df66432f15c9e11da601eef934 100644 --- a/src/examples/test_autodiff.cpp +++ b/src/examples/test_autodiff.cpp @@ -73,11 +73,11 @@ int main(int argc, char** argv) // INITIALIZATION ============================================================================================ //init random generators - WolfScalar odom_std_factor = 0.5; - WolfScalar gps_std = 1; + Scalar odom_std_factor = 0.5; + Scalar gps_std = 1; std::default_random_engine generator(1); - std::normal_distribution<WolfScalar> distribution_odom(0.0, odom_std_factor); //odometry noise - std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise + std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise + std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise //init google log //google::InitGoogleLogging(argv[0]); @@ -295,7 +295,7 @@ int main(int argc, char** argv) // std::vector<double> landmark_vector; // for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++) // { -// WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); +// Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); // landmark_vector.push_back(*position_ptr); //x // landmark_vector.push_back(*(position_ptr + 1)); //y // landmark_vector.push_back(0.2); //z @@ -348,7 +348,7 @@ int main(int argc, char** argv) // std::vector<double> landmark_vector; // for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++) // { -// WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); +// Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); // landmark_vector.push_back(*position_ptr); //x // landmark_vector.push_back(*(position_ptr + 1)); //y // landmark_vector.push_back(0.2); //z diff --git a/src/examples/test_capture_laser_2D.cpp b/src/examples/test_capture_laser_2D.cpp index 64318628898f470bc9e65b951064d0590bb9f3fe..5d64d279e7e4723aafda319bef5e52a88a5a6d05 100644 --- a/src/examples/test_capture_laser_2D.cpp +++ b/src/examples/test_capture_laser_2D.cpp @@ -101,7 +101,7 @@ int main(int argc, char *argv[]) //init a noise generator std::default_random_engine generator(1); - std::normal_distribution<WolfScalar> distribution_range(0.,device.getRangeStdDev()); //odometry noise + std::normal_distribution<Scalar> distribution_range(0.,device.getRangeStdDev()); //odometry noise //Create a Capture object CaptureLaser2D capture(time_stamp, &device, ranges); diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index a1b5c86c8302e1fbc566d568ca321483b0c30d42..99797393a659507101fe30dd64a0164b1a703066 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -72,11 +72,11 @@ int main(int argc, char** argv) // INITIALIZATION ============================================================================================ //init random generators - WolfScalar odom_std_factor = 0.1; - WolfScalar gps_std = 1; + Scalar odom_std_factor = 0.1; + Scalar gps_std = 1; std::default_random_engine generator(1); - std::normal_distribution<WolfScalar> distribution_odom(0.0, odom_std_factor); //odometry noise - std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise + std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise + std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise //init google log //google::InitGoogleLogging(argv[0]); @@ -248,7 +248,7 @@ int main(int argc, char** argv) std::vector<double> landmark_vector; for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++) { - WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); + Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); landmark_vector.push_back(*position_ptr); //x landmark_vector.push_back(*(position_ptr + 1)); //y landmark_vector.push_back(0.2); //z @@ -301,7 +301,7 @@ int main(int argc, char** argv) std::vector<double> landmark_vector; for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++) { - WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); + Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); landmark_vector.push_back(*position_ptr); //x landmark_vector.push_back(*(position_ptr + 1)); //y landmark_vector.push_back(0.2); //z diff --git a/src/examples/test_eigen_quaternion.cpp b/src/examples/test_eigen_quaternion.cpp index 9eca8b910e5717e44fb920114afc550e8dff47fa..cea2324871950a1cf754430a2435449a8e061bb9 100644 --- a/src/examples/test_eigen_quaternion.cpp +++ b/src/examples/test_eigen_quaternion.cpp @@ -14,7 +14,7 @@ int main() std::cout << std::endl << "Eigen Quatenrnion test" << std::endl; - WolfScalar q1[4]; + Scalar q1[4]; Eigen::Map<Eigen::Quaternions> q1_map(q1); //try to find out how eigen sorts storage (real part tail or head ? ) diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp index 0919eea9056c6e7bafc96fca7995ea1c9a6c0f82..575ed417ae0e87ac6fe58ce71e051c2a575d7bd1 100644 --- a/src/examples/test_image.cpp +++ b/src/examples/test_image.cpp @@ -141,7 +141,7 @@ int main(int argc, char** argv) capture >> frame; if (f>1){ // check if consecutive images are different - WolfScalar diff = cv::norm(frame, last_frame, cv::NORM_L1); + Scalar diff = cv::norm(frame, last_frame, cv::NORM_L1); std::cout << "test_image: Image increment: " << diff << std::endl; } diff --git a/src/examples/test_motion.cpp b/src/examples/test_motion.cpp index 43fec51c48044143c72dcde338a72d362068d814..139a7d8424c4fe5ca98bbc98b3e4dc2ca165f904 100644 --- a/src/examples/test_motion.cpp +++ b/src/examples/test_motion.cpp @@ -31,7 +31,7 @@ int main() TimeStamp t0, t; t0.setToNow(); t = t0; - WolfScalar dt = .01; + Scalar dt = .01; // robot state blocks: origin StateBlock sb_pos(Eigen::Vector3s::Zero()); @@ -75,7 +75,7 @@ int main() std::cout << "\nQuery states at asynchronous time values..." << std::endl; t = t0; - WolfScalar dt_2 = dt/2; + Scalar dt_2 = dt/2; dt = 0.0045; // new dt for (int i = 1; i <= 20; i++) { @@ -91,15 +91,15 @@ int main() std::cout << "\n\nTrying a std::map as the buffer container <-- NOT WORKING: need exact key" << std::endl; - WolfScalar x; - std::map<TimeStamp, WolfScalar> buffer_map; + Scalar x; + std::map<TimeStamp, Scalar> buffer_map; t.set(0); x = 0; for (double i = 1; i<=10; i++) { t.set(i/5); x++; - buffer_map.insert(std::pair<TimeStamp,WolfScalar>(t,x)); + buffer_map.insert(std::pair<TimeStamp,Scalar>(t,x)); std::cout << "insert (ts,x) = (" << t.get() << "," << x << ")" << std::endl; } for (double i = 1; i<=8; i++) @@ -114,7 +114,7 @@ int main() std::cout << "\n\nTrying a std::list and std::find_if as the buffer container <-- WORKING: can use comparator '<' for evaluating key" << std::endl; - typedef std::pair<TimeStamp, WolfScalar> Pair; + typedef std::pair<TimeStamp, Scalar> Pair; typedef std::list<Pair> PairsList; PairsList buffer_list; diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp index b0bc70a6b390ecf20f008435959ed638e69fb572..5f33e257bf32095278a622ec3c3d085185825589 100644 --- a/src/examples/test_wolf_imported_graph.cpp +++ b/src/examples/test_wolf_imported_graph.cpp @@ -20,10 +20,10 @@ namespace wolf{ // inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers -void insertSparseBlock(const Eigen::SparseMatrix<WolfScalar>& ins, Eigen::SparseMatrix<WolfScalar>& original, const unsigned int& row, const unsigned int& col) +void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col) { for (int k=0; k<ins.outerSize(); ++k) - for (Eigen::SparseMatrix<WolfScalar>::InnerIterator iti(ins,k); iti; ++iti) + for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti) original.coeffRef(iti.row() + row, iti.col() + col) = iti.value(); original.makeCompressed(); @@ -54,7 +54,7 @@ int main(int argc, char** argv) clock_t t1; ceres::Solver::Summary summary_full, summary_prun; Eigen::MatrixXs Sigma_ii(3,3), Sigma_ij(3,3), Sigma_jj(3,3), Sigma_z(3,3), Ji(3,3), Jj(3,3); - WolfScalar xi, yi, thi, si, ci, xj, yj; + Scalar xi, yi, thi, si, ci, xj, yj; // loading variables std::map<unsigned int, FrameBase*> index_2_frame_ptr_full; @@ -66,7 +66,7 @@ int main(int argc, char** argv) WolfProblem* wolf_problem_prun = new WolfProblem(FRM_PO_2D); SensorBase* sensor = new SensorBase(SEN_ODOM_2D, new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2); - Eigen::SparseMatrix<WolfScalar> Lambda(0,0); + Eigen::SparseMatrix<Scalar> Lambda(0,0); // Ceres wrapper ceres::Solver::Options ceres_options; @@ -275,13 +275,13 @@ int main(int argc, char** argv) //std::cout << "information " << std::endl << edge_information << std::endl; //std::cout << "covariance " << std::endl << constraint_ptr_prun->getMeasurementCovariance() << std::endl; - WolfScalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr()); - WolfScalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1); - WolfScalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr()); - WolfScalar si = sin(thi); - WolfScalar ci = cos(thi); - WolfScalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr()); - WolfScalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1); + Scalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr()); + Scalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1); + Scalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr()); + Scalar si = sin(thi); + Scalar ci = cos(thi); + Scalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr()); + Scalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1); Eigen::MatrixXs Ji(3,3), Jj(3,3); Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci, si,-ci,-(xj-xi)*ci-(yj-yi)*si, @@ -291,7 +291,7 @@ int main(int argc, char** argv) 0, 0, 1; //std::cout << "Ji" << std::endl << Ji << std::endl; //std::cout << "Jj" << std::endl << Jj << std::endl; - Eigen::SparseMatrix<WolfScalar> DeltaLambda(Lambda.rows(), Lambda.cols()); + Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols()); insertSparseBlock((Ji.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_old*3, edge_old*3); insertSparseBlock((Jj.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_new*3, edge_new*3); insertSparseBlock((Ji.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_old*3, edge_new*3); @@ -317,7 +317,7 @@ int main(int argc, char** argv) initial_covariance_full->process(); initial_covariance_prun->process(); //std::cout << "initial covariance: constraint " << initial_covariance_prun->getFeatureListPtr()->front()->getConstraintFromListPtr()->front()->nodeId() << std::endl << initial_covariance_prun->getFeatureListPtr()->front()->getMeasurementCovariance() << std::endl; - Eigen::SparseMatrix<WolfScalar> DeltaLambda(Lambda.rows(), Lambda.cols()); + Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols()); insertSparseBlock((Eigen::Matrix3s::Identity() * 100).sparseView(), DeltaLambda, 0, 0); Lambda = Lambda + DeltaLambda; @@ -332,7 +332,7 @@ int main(int argc, char** argv) wolf_problem_prun->getTrajectoryPtr()->getConstraintList(constraints); // Manual covariance computation t1 = clock(); - Eigen::SimplicialLLT<Eigen::SparseMatrix<WolfScalar>> chol(Lambda); // performs a Cholesky factorization of A + Eigen::SimplicialLLT<Eigen::SparseMatrix<Scalar>> chol(Lambda); // performs a Cholesky factorization of A Eigen::MatrixXs Sigma = chol.solve(Eigen::MatrixXs::Identity(Lambda.rows(), Lambda.cols())); double t_sigma_manual = ((double) clock() - t1) / CLOCKS_PER_SEC; //std::cout << "Lambda" << std::endl << Lambda << std::endl; @@ -398,7 +398,7 @@ int main(int argc, char** argv) //std::cout << "denominador : " << std::endl << Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl; // Information gain - WolfScalar IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() ); + Scalar IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() ); //std::cout << "IG = " << IG << std::endl; if (IG < 2) diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp index 7d21eb97a9760e55d28dff5b97c3cd7454247f6e..e92064788fc3740bce8d237f857fc59413f21f5b 100644 --- a/src/examples/test_wolf_prunning.cpp +++ b/src/examples/test_wolf_prunning.cpp @@ -21,10 +21,10 @@ namespace wolf{ // inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers -void insertSparseBlock(const Eigen::SparseMatrix<WolfScalar>& ins, Eigen::SparseMatrix<WolfScalar>& original, const unsigned int& row, const unsigned int& col) +void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatrix<Scalar>& original, const unsigned int& row, const unsigned int& col) { for (int k=0; k<ins.outerSize(); ++k) - for (Eigen::SparseMatrix<WolfScalar>::InnerIterator iti(ins,k); iti; ++iti) + for (Eigen::SparseMatrix<Scalar>::InnerIterator iti(ins,k); iti; ++iti) original.coeffRef(iti.row() + row, iti.col() + col) = iti.value(); original.makeCompressed(); @@ -65,7 +65,7 @@ int main(int argc, char** argv) jacobians.push_back(Eigen::MatrixXs::Zero(3,1)); jacobians.push_back(Eigen::MatrixXs::Zero(3,2)); jacobians.push_back(Eigen::MatrixXs::Zero(3,1)); - WolfScalar xi, yi, thi, si, ci, xj, yj; + Scalar xi, yi, thi, si, ci, xj, yj; double t_sigma_manual = 0; // loading variables @@ -78,11 +78,11 @@ int main(int argc, char** argv) WolfProblem* wolf_problem_prun = new WolfProblem(FRM_PO_2D); SensorBase* sensor = new SensorBase(SEN_ODOM_2D, new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2); - Eigen::SparseMatrix<WolfScalar> Lambda(0,0); + Eigen::SparseMatrix<Scalar> Lambda(0,0); // prunning std::list<ConstraintBase*> ordered_ctr_ptr; - std::list<WolfScalar> ordered_ig; + std::list<Scalar> ordered_ig; // Ceres wrapper ceres::Solver::Options ceres_options; @@ -291,13 +291,13 @@ int main(int argc, char** argv) //std::cout << "covariance " << std::endl << constraint_ptr_prun->getMeasurementCovariance() << std::endl; t1 = clock(); - WolfScalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr()); - WolfScalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1); - WolfScalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr()); - WolfScalar si = sin(thi); - WolfScalar ci = cos(thi); - WolfScalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr()); - WolfScalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1); + Scalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr()); + Scalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1); + Scalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr()); + Scalar si = sin(thi); + Scalar ci = cos(thi); + Scalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr()); + Scalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1); Eigen::MatrixXs Ji(3,3), Jj(3,3); Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci, si,-ci,-(xj-xi)*ci-(yj-yi)*si, @@ -307,7 +307,7 @@ int main(int argc, char** argv) 0, 0, 1; //std::cout << "Ji" << std::endl << Ji << std::endl; //std::cout << "Jj" << std::endl << Jj << std::endl; - Eigen::SparseMatrix<WolfScalar> DeltaLambda(Lambda.rows(), Lambda.cols()); + Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols()); insertSparseBlock((Ji.transpose() * edge_information * Ji).sparseView(), DeltaLambda, edge_old*3, edge_old*3); insertSparseBlock((Jj.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_new*3, edge_new*3); insertSparseBlock((Ji.transpose() * edge_information * Jj).sparseView(), DeltaLambda, edge_old*3, edge_new*3); @@ -335,7 +335,7 @@ int main(int argc, char** argv) initial_covariance_prun->process(); //std::cout << "initial covariance: constraint " << initial_covariance_prun->getFeatureListPtr()->front()->getConstraintFromListPtr()->front()->nodeId() << std::endl << initial_covariance_prun->getFeatureListPtr()->front()->getMeasurementCovariance() << std::endl; t1 = clock(); - Eigen::SparseMatrix<WolfScalar> DeltaLambda(Lambda.rows(), Lambda.cols()); + Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols()); insertSparseBlock((Eigen::Matrix3s::Identity() * 100).sparseView(), DeltaLambda, 0, 0); Lambda = Lambda + DeltaLambda; t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC; @@ -351,7 +351,7 @@ int main(int argc, char** argv) wolf_problem_prun->getTrajectoryPtr()->getConstraintList(constraints); // Manual covariance computation t1 = clock(); - Eigen::SimplicialLLT<Eigen::SparseMatrix<WolfScalar>> chol(Lambda); // performs a Cholesky factorization of A + Eigen::SimplicialLLT<Eigen::SparseMatrix<Scalar>> chol(Lambda); // performs a Cholesky factorization of A Eigen::MatrixXs Sigma = chol.solve(Eigen::MatrixXs::Identity(Lambda.rows(), Lambda.cols())); t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC; //std::cout << "Lambda" << std::endl << Lambda << std::endl; @@ -424,7 +424,7 @@ int main(int argc, char** argv) // std::cout << "J4" << std::endl << J4 << std::endl; // Information gain - WolfScalar IG_new = 0.5 * log( Sigma_z.determinant() / + Scalar IG_new = 0.5 * log( Sigma_z.determinant() / ( Sigma_z - (J1 * Sigma_11 * J1.transpose() + J1 * Sigma_12 * J2.transpose() + J1 * Sigma_13 * J3.transpose() + @@ -504,7 +504,7 @@ int main(int argc, char** argv) //std::cout << "denominador : " << std::endl << Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose()) << std::endl; // Information gain - WolfScalar IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() ); + Scalar IG = 0.5 * log( Sigma_z.determinant() / (Sigma_z - (Ji * Sigma_ii * Ji.transpose() + Jj * Sigma_jj * Jj.transpose() + Ji * Sigma_ij * Jj.transpose() + Jj * Sigma_ij.transpose() * Ji.transpose())).determinant() ); // std::cout << "part = " << std::endl << (Ji * Sigma_ii * Ji.transpose() + // Jj * Sigma_jj * Jj.transpose() + diff --git a/src/feature_base.h b/src/feature_base.h index ef0fe032b5138839c08c482f6952916bf71028b1..c1505fce202fe9206be3b5c2debd3dcde4f20e63 100644 --- a/src/feature_base.h +++ b/src/feature_base.h @@ -70,7 +70,7 @@ class FeatureBase : public NodeConstrained<CaptureBase,ConstraintBase> * * WARNING: To be fast, it does not check that index _ii is smaller than dimension. **/ - WolfScalar getMeasurement(unsigned int _ii) const; + Scalar getMeasurement(unsigned int _ii) const; /** \brief Returns a reference to the feature measurement covariance **/ @@ -91,7 +91,7 @@ inline CaptureBase* FeatureBase::getCapturePtr() const return upperNodePtr(); } -inline WolfScalar FeatureBase::getMeasurement(unsigned int _ii) const +inline Scalar FeatureBase::getMeasurement(unsigned int _ii) const { return measurement_(_ii); } diff --git a/src/feature_corner_2D.cpp b/src/feature_corner_2D.cpp index ce4a025e13ff76d52e2e17baaa4b7d490cdf63a9..37c8c4c3c3979c6fb9f8835535917e63a4d020fd 100644 --- a/src/feature_corner_2D.cpp +++ b/src/feature_corner_2D.cpp @@ -14,7 +14,7 @@ FeatureCorner2D::~FeatureCorner2D() // } -WolfScalar FeatureCorner2D::getAperture() const +Scalar FeatureCorner2D::getAperture() const { return measurement_(3); } diff --git a/src/feature_corner_2D.h b/src/feature_corner_2D.h index cab19ab23aec20004759d27277a7cc9761a865a8..8e72baf91a0a4bad125ca34fb60f0f2af87359f0 100644 --- a/src/feature_corner_2D.h +++ b/src/feature_corner_2D.h @@ -18,7 +18,7 @@ class FeatureCorner2D : public FeatureBase * * constructor */ - FeatureCorner2D(const Eigen::Vector4s & _measurement, const Eigen::Matrix4s & _meas_covariance); //TODO: add const WolfScalar& aperture); + FeatureCorner2D(const Eigen::Vector4s & _measurement, const Eigen::Matrix4s & _meas_covariance); //TODO: add const Scalar& aperture); /** \brief Default destructor (not recommended) * @@ -32,7 +32,7 @@ class FeatureCorner2D : public FeatureBase * Returns aperture * **/ - WolfScalar getAperture() const; + Scalar getAperture() const; }; diff --git a/src/feature_gps_pseudorange.cpp b/src/feature_gps_pseudorange.cpp index 096038437f27d1140a6798930c568bb1204b45d0..4042b4f3d285c5cc369b7faae5a3e99a191a1a40 100644 --- a/src/feature_gps_pseudorange.cpp +++ b/src/feature_gps_pseudorange.cpp @@ -2,7 +2,7 @@ namespace wolf { -FeatureGPSPseudorange::FeatureGPSPseudorange(Eigen::Vector3s &_sat_position, WolfScalar _pseudorange, WolfScalar _covariance) : +FeatureGPSPseudorange::FeatureGPSPseudorange(Eigen::Vector3s &_sat_position, Scalar _pseudorange, Scalar _covariance) : FeatureBase(FEAT_GPS_PR, Eigen::VectorXs::Constant(1,_pseudorange), Eigen::MatrixXs::Identity(1,1)*_covariance), sat_position_(_sat_position), pseudorange_(_pseudorange) @@ -19,7 +19,7 @@ FeatureGPSPseudorange::~FeatureGPSPseudorange() } -WolfScalar FeatureGPSPseudorange::getPseudorange() const +Scalar FeatureGPSPseudorange::getPseudorange() const { return pseudorange_; } diff --git a/src/feature_gps_pseudorange.h b/src/feature_gps_pseudorange.h index 922ad001d57ecd4f26ea1f852f34f5f6108eaaaa..431efb32ab5a47db507d7c36a0a5d2f02107b2cc 100644 --- a/src/feature_gps_pseudorange.h +++ b/src/feature_gps_pseudorange.h @@ -18,10 +18,10 @@ class FeatureGPSPseudorange : public FeatureBase { protected: Eigen::Vector3s sat_position_; - WolfScalar pseudorange_; + Scalar pseudorange_; public: - FeatureGPSPseudorange(Eigen::Vector3s& _sat_position, WolfScalar _pseudorange, WolfScalar _covariance); + FeatureGPSPseudorange(Eigen::Vector3s& _sat_position, Scalar _pseudorange, Scalar _covariance); /** \brief Default destructor (not recommended) * @@ -32,7 +32,7 @@ public: const Eigen::Vector3s & getSatPosition() const; - WolfScalar getPseudorange() const; + Scalar getPseudorange() const; }; diff --git a/src/feature_point_image.h b/src/feature_point_image.h index 35c80eca6123cbccb52a81680af58e34582faa0f..8ae3546441c8f09ebe40c3c9256f977e94871d58 100644 --- a/src/feature_point_image.h +++ b/src/feature_point_image.h @@ -63,8 +63,8 @@ class FeaturePointImage : public FeatureBase void setIsKnown(bool _is_known); Eigen::VectorXs & getMeasurement(){ - measurement_(0) = WolfScalar(keypoint_.pt.x); - measurement_(1) = WolfScalar(keypoint_.pt.y); + measurement_(0) = Scalar(keypoint_.pt.x); + measurement_(1) = Scalar(keypoint_.pt.y); return measurement_; } diff --git a/src/landmark_base.h b/src/landmark_base.h index 59481577d7d725d7fdb87c0af20ed009b3349fe4..e7fd9e4e8da91bf1b85298becdda67bbd66111d5 100644 --- a/src/landmark_base.h +++ b/src/landmark_base.h @@ -98,7 +98,7 @@ class LandmarkBase : public NodeConstrained<MapBase, NodeTerminus> * * WARNING: To be fast, it does not check that index _ii is smaller than dimension. **/ - WolfScalar getDescriptor(unsigned int _ii) const; + Scalar getDescriptor(unsigned int _ii) const; /** \brief Return the type of the landmark **/ @@ -149,7 +149,7 @@ inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor) descriptor_ = _descriptor; } -inline WolfScalar LandmarkBase::getDescriptor(unsigned int _ii) const +inline Scalar LandmarkBase::getDescriptor(unsigned int _ii) const { return descriptor_(_ii); } diff --git a/src/landmark_container.cpp b/src/landmark_container.cpp index 3c70c794c72882157b58d044f31cf264f4fffc52..0bd54d60b42e19f5fbd16abc71c11898102acd8a 100644 --- a/src/landmark_container.cpp +++ b/src/landmark_container.cpp @@ -4,7 +4,7 @@ namespace wolf { -LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _witdh, const WolfScalar& _length) : +LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _witdh, const Scalar& _length) : LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr), corners_(3,4) { @@ -17,7 +17,7 @@ LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, con M_PI / 4, 3 * M_PI / 4,-3 * M_PI / 4,-M_PI / 4; } -LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh, const WolfScalar& _length) : +LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const Scalar& _witdh, const Scalar& _length) : LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr), corners_(3,4) { @@ -83,7 +83,7 @@ LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, con std::cout << "Container center pose... " << container_position.transpose() << " " << container_orientation.transpose() << std::endl; } -//LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh, const WolfScalar& _length) : +//LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh, const Scalar& _length) : // LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr), // corners_(3,4) //{ @@ -164,12 +164,12 @@ LandmarkContainer::~LandmarkContainer() // tODO delete corners } -WolfScalar LandmarkContainer::getWidth() const +Scalar LandmarkContainer::getWidth() const { return descriptor_(0); } -WolfScalar LandmarkContainer::getLength() const +Scalar LandmarkContainer::getLength() const { return descriptor_(1); } diff --git a/src/landmark_container.h b/src/landmark_container.h index 02bc307d15fda36e4fb41fd226931909dd644e25..869279a01cff9ba2deb131b708cc6261c48e4f3e 100644 --- a/src/landmark_container.h +++ b/src/landmark_container.h @@ -26,7 +26,7 @@ class LandmarkContainer : public LandmarkBase * \param _length descriptor of the landmark: container length * **/ - LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2); + LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _witdh=2.44, const Scalar& _length=12.2); /** \brief Constructor with type, time stamp and the position state pointer * @@ -51,7 +51,7 @@ class LandmarkContainer : public LandmarkBase * \param _length descriptor of the landmark: container length * **/ - LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2); + LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const Scalar& _witdh=2.44, const Scalar& _length=12.2); /** \brief Constructor with type, time stamp and the position state pointer * @@ -66,7 +66,7 @@ class LandmarkContainer : public LandmarkBase * \param _length descriptor of the landmark: container length * **/ - //LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2); + //LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh=2.44, const Scalar& _length=12.2); /** \brief Default destructor (not recommended) * @@ -80,14 +80,14 @@ class LandmarkContainer : public LandmarkBase * Returns the container width * **/ - WolfScalar getWidth() const; + Scalar getWidth() const; /** \brief Returns the container length * * Returns the container length * **/ - WolfScalar getLength() const; + Scalar getLength() const; /** \brief Returns the container corners in container coordinates * diff --git a/src/landmark_corner_2D.cpp b/src/landmark_corner_2D.cpp index f6b32607ce483ccaba35e5e96fe4f7b8ca20fc61..1a6f1eacaea17f08fbeb10226a7f0c0b19702712 100644 --- a/src/landmark_corner_2D.cpp +++ b/src/landmark_corner_2D.cpp @@ -3,7 +3,7 @@ namespace wolf { -LandmarkCorner2D::LandmarkCorner2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _aperture) : +LandmarkCorner2D::LandmarkCorner2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _aperture) : LandmarkBase(LANDMARK_CORNER, _p_ptr, _o_ptr) { setDescriptor(Eigen::VectorXs::Constant(1,_aperture)); @@ -15,7 +15,7 @@ LandmarkCorner2D::~LandmarkCorner2D() } -WolfScalar LandmarkCorner2D::getAperture() const +Scalar LandmarkCorner2D::getAperture() const { return descriptor_(0); } diff --git a/src/landmark_corner_2D.h b/src/landmark_corner_2D.h index 573941bdb1b12f3c93e0643649daa3369d18fd03..73156a6e04ef13566cb1de65652eacba6de957a4 100644 --- a/src/landmark_corner_2D.h +++ b/src/landmark_corner_2D.h @@ -25,7 +25,7 @@ class LandmarkCorner2D : public LandmarkBase * \param _aperture descriptor of the landmark: aperture of the corner * **/ - LandmarkCorner2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _aperture=0); + LandmarkCorner2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _aperture=0); /** \brief Default destructor (not recommended) * @@ -39,7 +39,7 @@ class LandmarkCorner2D : public LandmarkBase * Returns aperture * **/ - WolfScalar getAperture() const; + Scalar getAperture() const; }; diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp index 329435599ef18bfc399d43060fd556cae7ae6a59..1df0975b4bb79483e1c5e79b1e115a9d39649272 100644 --- a/src/local_parametrization_homogeneous.cpp +++ b/src/local_parametrization_homogeneous.cpp @@ -37,7 +37,7 @@ bool LocalParametrizationHomogeneous::plus(const Eigen::Map<const Eigen::VectorX Vector3s axis = _delta / norm_delta; // express delta as a quaternion - Quaternions dq(AngleAxis<WolfScalar>(norm_delta, axis)); + Quaternions dq(AngleAxis<Scalar>(norm_delta, axis)); // result as a homogeneous point -- we use the quaternion product for keeping in the 4-sphere _h_plus_delta = (dq * Map<const Quaternions>(_h.data())).coeffs(); diff --git a/src/local_parametrization_quaternion.cpp b/src/local_parametrization_quaternion.cpp index 2c18aefb2bf04977e516cfc417b069220c499869..dd9c22ec2b7fea541c7660f5340b7f51928f14be 100644 --- a/src/local_parametrization_quaternion.cpp +++ b/src/local_parametrization_quaternion.cpp @@ -32,7 +32,7 @@ bool LocalParametrizationQuaternion::plus(const Eigen::Map<const Eigen::VectorXs Vector3s axis = _delta_theta / angle; // express delta_theta as a quaternion using the angle-axis helper - Quaternions dq(AngleAxis<WolfScalar>(angle, axis)); + Quaternions dq(AngleAxis<Scalar>(angle, axis)); // result as a quaternion if (delta_reference_ == DQ_GLOBAL) diff --git a/src/processor_base.h b/src/processor_base.h index b285ebf9a5f1ab27616b226029874be30b928db1..a73dfc3c349e7390b0298042a16fc113f675b17f 100644 --- a/src/processor_base.h +++ b/src/processor_base.h @@ -41,7 +41,7 @@ class ProcessorBase : public NodeLinked<SensorBase, NodeTerminus> SensorBase* getSensorPtr(); - //virtual void newKeyFrameCallback(FrameBase* _new_key_frame_ptr, const WolfScalar& _time_tolerance) = 0; + //virtual void newKeyFrameCallback(FrameBase* _new_key_frame_ptr, const Scalar& _time_tolerance) = 0; private: ProcessorType type_; diff --git a/src/processor_brisk.cpp b/src/processor_brisk.cpp index 18c9c7a925d90cda38749b9b0df531295c488f24..e3164095952f63230b0aeb15b29534db5e24e0fa 100644 --- a/src/processor_brisk.cpp +++ b/src/processor_brisk.cpp @@ -157,7 +157,7 @@ unsigned int ProcessorBrisk::trackFeatures(const FeatureBaseList& _feature_list_ std::cout << "Number of features to track: " << _feature_list_in.size() << std::endl << std::endl; std::cout << "last*: " << last_ptr_ << " -- incoming*: " << incoming_ptr_ << std::endl; - WolfScalar diff = cv::norm(image_incoming_, image_last_, cv::NORM_L1); + Scalar diff = cv::norm(image_incoming_, image_last_, cv::NORM_L1); std::cout << "Distance between last and incoming images: " << diff << std::endl; for (auto feature_base_ptr : _feature_list_in) @@ -201,7 +201,7 @@ unsigned int ProcessorBrisk::trackFeatures(const FeatureBaseList& _feature_list_ _feature_list_out.push_back(incoming_point_ptr); _feature_matches[incoming_point_ptr] = FeatureMatch(feature_base_ptr, - 1 - (WolfScalar)(cv_matches[0].distance)/512); //FIXME: 512 is the maximum HAMMING distance + 1 - (Scalar)(cv_matches[0].distance)/512); //FIXME: 512 is the maximum HAMMING distance } for (unsigned int i = 0; i < candidate_keypoints.size(); i++) // TODO Arreglar todos los <= y -1 por < y nada. diff --git a/src/processor_brisk.h b/src/processor_brisk.h index 00d7f039a382d77df919e8ad8e52615f98d87171..cbc618f28595223295e7a60da9b93d81478fde8e 100644 --- a/src/processor_brisk.h +++ b/src/processor_brisk.h @@ -46,7 +46,7 @@ struct ImageTrackerParameters }descriptor; struct Matcher { - WolfScalar max_similarity_distance; ///< 0: perfect match; 1 or -1: awful match; out of [-1,1]: error + Scalar max_similarity_distance; ///< 0: perfect match; 1 or -1: awful match; out of [-1,1]: error }matcher; struct Adtive_search { diff --git a/src/processor_gps.cpp b/src/processor_gps.cpp index 47659402d86659343db026eaecb985f7f553e43c..c99b7721396435423c0e742fc37e1ee32ba7734f 100644 --- a/src/processor_gps.cpp +++ b/src/processor_gps.cpp @@ -40,7 +40,7 @@ void ProcessorGPS::process(CaptureBase* _capture_ptr) for(unsigned int i = 0; i < obs.measurements_.size(); ++i) { Eigen::Vector3s sat_pos = obs.measurements_[i].sat_position_; - WolfScalar pr = obs.measurements_[i].pseudorange_; + Scalar pr = obs.measurements_[i].pseudorange_; capture_gps_ptr_->addFeature(new FeatureGPSPseudorange(sat_pos, pr, gps_covariance_)); } diff --git a/src/processor_gps.h b/src/processor_gps.h index 9c0ed0dc447c00bcf5e73ff5ebb957fd9b176327..5fe4a732b9b20155e68eaf74cf63774bb1e96368 100644 --- a/src/processor_gps.h +++ b/src/processor_gps.h @@ -21,7 +21,7 @@ class ProcessorGPS : public ProcessorBase //SensorGPS* sensor_gps_ptr_; //specific pointer to sensor gps object CaptureGPS* capture_gps_ptr_; - WolfScalar gps_covariance_; + Scalar gps_covariance_; public: diff --git a/src/processor_laser_2D.cpp b/src/processor_laser_2D.cpp index b54e1f9a7ba1907d5f4d0cfd3ca8ebde6c3e0637..9ca54258b08661ccfcd1e3794e41fdb5c410dc2c 100644 --- a/src/processor_laser_2D.cpp +++ b/src/processor_laser_2D.cpp @@ -87,10 +87,10 @@ void ProcessorLaser2D::createFeatures(std::list<laserscanutils::Corner> & _corne meas(3) = corner_it->aperture_; // TODO: maybe in line object? - WolfScalar L1 = corner_it->line_1_.length(); - WolfScalar L2 = corner_it->line_2_.length(); - WolfScalar cov_angle_line1 = 12 * corner_it->line_1_.error_ / (pow(L1,2) * (pow(corner_it->line_1_.np_,3)-pow(corner_it->line_1_.np_,2))); - WolfScalar cov_angle_line2 = 12 * corner_it->line_2_.error_ / (pow(L2,2) * (pow(corner_it->line_2_.np_,3)-pow(corner_it->line_2_.np_,2))); + Scalar L1 = corner_it->line_1_.length(); + Scalar L2 = corner_it->line_2_.length(); + Scalar cov_angle_line1 = 12 * corner_it->line_1_.error_ / (pow(L1,2) * (pow(corner_it->line_1_.np_,3)-pow(corner_it->line_1_.np_,2))); + Scalar cov_angle_line2 = 12 * corner_it->line_2_.error_ / (pow(L2,2) * (pow(corner_it->line_2_.np_,3)-pow(corner_it->line_2_.np_,2))); //init cov in corner coordinates @@ -103,7 +103,7 @@ void ProcessorLaser2D::createFeatures(std::list<laserscanutils::Corner> & _corne //std::cout << cov_mat << std::endl; // Rotate covariance - R.topLeftCorner<2,2>() = Eigen::Rotation2D<WolfScalar>(corner_it->orientation_).matrix(); + R.topLeftCorner<2,2>() = Eigen::Rotation2D<Scalar>(corner_it->orientation_).matrix(); cov_mat.topLeftCorner<3,3>() = R.transpose() * cov_mat.topLeftCorner<3,3>() * R; //std::cout << "rotated covariance: " << std::endl; @@ -119,11 +119,11 @@ void ProcessorLaser2D::establishConstraintsMHTree() if (capture_laser_ptr_->getFeatureListPtr()->size() != 0) { //local declarations - WolfScalar prob, dm2; + Scalar prob, dm2; unsigned int ii, jj; std::map<unsigned int, unsigned int> ft_lk_pairs; std::vector<bool> associated_mask; - WolfScalar sq24 = sqrt(2) / 4; + Scalar sq24 = sqrt(2) / 4; Eigen::MatrixXs corners(3,4); // Center seen from corners (see LandmarkContainer.h) // A // B // C // D @@ -187,7 +187,7 @@ void ProcessorLaser2D::establishConstraintsMHTree() if (fabs(pi2pi(((FeatureCorner2D*)(*feature_it))->getAperture() - 3 * M_PI / 2)) < MAX_ACCEPTED_APERTURE_DIFF) { // Rotate corners (seen from feature coordinates) - //rotated_corners.topRows(2) = Eigen::Rotation2D<WolfScalar>(expected_features[*landmark_it](2)).matrix() * corners.topRows(2); + //rotated_corners.topRows(2) = Eigen::Rotation2D<Scalar>(expected_features[*landmark_it](2)).matrix() * corners.topRows(2); rotated_corners = corners; squared_mahalanobis_distances = computeSquaredMahalanobisDistances((*feature_it), expected_features[*landmark_it], expected_features_covs[*landmark_it], rotated_corners); @@ -270,21 +270,21 @@ void ProcessorLaser2D::establishConstraintsMHTree() // Global transformation Eigen::Vector2s t_robot = capture_laser_ptr_->getFramePtr()->getPPtr()->getVector().head(2); - Eigen::Matrix2s R_robot = Eigen::Rotation2D<WolfScalar>(*(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr())).matrix(); - WolfScalar robot_orientation = *(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr()); + Eigen::Matrix2s R_robot = Eigen::Rotation2D<Scalar>(*(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr())).matrix(); + Scalar robot_orientation = *(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr()); // Sensor transformation Eigen::Vector2s t_sensor = getSensorPtr()->getPPtr()->getVector().head(2); - Eigen::Rotation2D<WolfScalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) ); + Eigen::Rotation2D<Scalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) ); Eigen::Matrix2s R_sensor = S_R.matrix(); - WolfScalar sensor_orientation = *(capture_laser_ptr_->getSensorPtr()->getOPtr()->getPtr()); + Scalar sensor_orientation = *(capture_laser_ptr_->getSensorPtr()->getOPtr()->getPtr()); // NEW CORNERS for (auto it=new_corner_landmarks.begin(); it!=new_corner_landmarks.end();it++) { //get feature on sensor frame const Eigen::Vector2s& feature_position = (*it)->getMeasurement().head(2); - const WolfScalar& feature_orientation = (*it)->getMeasurement()(2); + const Scalar& feature_orientation = (*it)->getMeasurement()(2); //translate feature position and orientation to world (global) feature_global_pose.head(2) = R_robot * (R_sensor * feature_position + t_sensor) + t_robot; @@ -298,7 +298,7 @@ void ProcessorLaser2D::establishConstraintsMHTree() { //get feature on sensor frame const Eigen::Vector2s& feature_position = new_container_landmarks.at(i).first->getMeasurement().head(2); - const WolfScalar& feature_orientation = new_container_landmarks.at(i).first->getMeasurement()(2); + const Scalar& feature_orientation = new_container_landmarks.at(i).first->getMeasurement()(2); //translate feature position and orientation to world (global) feature_global_pose.head(2) = R_robot * (R_sensor * feature_position + t_sensor) + t_robot; @@ -319,18 +319,18 @@ void ProcessorLaser2D::computeExpectedFeature(LandmarkBase* _landmark_ptr, Eigen FrameBase* frame_ptr = capture_laser_ptr_->getFramePtr(); Eigen::Vector2s p_robot = frame_ptr->getPPtr()->getVector(); - WolfScalar o_robot = *(frame_ptr->getOPtr()->getPtr()); + Scalar o_robot = *(frame_ptr->getOPtr()->getPtr()); Eigen::Vector2s p_sensor = getSensorPtr()->getPPtr()->getVector().head(2); - WolfScalar o_sensor = *(getSensorPtr()->getOPtr()->getPtr()); - Eigen::Rotation2D<WolfScalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) ); + Scalar o_sensor = *(getSensorPtr()->getOPtr()->getPtr()); + Eigen::Rotation2D<Scalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) ); Eigen::Matrix2s R_sensor = S_R.matrix(); Eigen::Vector2s p_landmark = _landmark_ptr->getPPtr()->getVector(); - WolfScalar o_landmark = *(_landmark_ptr->getOPtr()->getPtr()); + Scalar o_landmark = *(_landmark_ptr->getOPtr()->getPtr()); - WolfScalar o_rs = o_robot + o_sensor; // Sum of theta_sensor and theta_robot - Eigen::Matrix2s R_sr = Eigen::Rotation2D<WolfScalar>(o_rs).matrix(); // Sum of theta_sensor and theta_robot + Scalar o_rs = o_robot + o_sensor; // Sum of theta_sensor and theta_robot + Eigen::Matrix2s R_sr = Eigen::Rotation2D<Scalar>(o_rs).matrix(); // Sum of theta_sensor and theta_robot Eigen::Vector2s p_robot_landmark = p_robot - p_landmark; @@ -349,7 +349,7 @@ void ProcessorLaser2D::computeExpectedFeature(LandmarkBase* _landmark_ptr, Eigen // ------------------------ Sigma // JACOBIAN - Eigen::Matrix<WolfScalar, 3, 6> Jlr = Eigen::Matrix<WolfScalar, 3, 6>::Zero(); + Eigen::Matrix<Scalar, 3, 6> Jlr = Eigen::Matrix<Scalar, 3, 6>::Zero(); Jlr.block<2,2>(0,0) = -R_sr.transpose(); Jlr(2,2) = -1; Jlr.block<2,2>(0,3) = R_sr.transpose(); @@ -383,22 +383,22 @@ Eigen::VectorXs ProcessorLaser2D::computeSquaredMahalanobisDistances(const Featu FrameBase* frame_ptr = capture_laser_ptr_->getFramePtr(); Eigen::Vector2s p_robot = frame_ptr->getPPtr()->getVector(); - WolfScalar o_robot = *(frame_ptr->getOPtr()->getPtr()); + Scalar o_robot = *(frame_ptr->getOPtr()->getPtr()); Eigen::Vector2s p_sensor = getSensorPtr()->getPPtr()->getVector().head(2); - WolfScalar o_sensor = *(getSensorPtr()->getOPtr()->getPtr()); + Scalar o_sensor = *(getSensorPtr()->getOPtr()->getPtr()); // Eigen::Matrix2s R_sensor = getSensorPtr()->getRotationMatrix2D(); - Eigen::Rotation2D<WolfScalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) ); + Eigen::Rotation2D<Scalar> S_R( getSensorPtr()->getOPtr()->getVector()(0) ); Eigen::Matrix2s R_sensor = S_R.matrix(); Eigen::Vector2s p_landmark = _landmark_ptr->getPPtr()->getVector(); - WolfScalar o_landmark = *(_landmark_ptr->getOPtr()->getPtr()); + Scalar o_landmark = *(_landmark_ptr->getOPtr()->getPtr()); const Eigen::Vector2s& p_feature = _feature_ptr->getMeasurement().head(2); - const WolfScalar& o_feature = _feature_ptr->getMeasurement()(2); + const Scalar& o_feature = _feature_ptr->getMeasurement()(2); - WolfScalar o_rs = o_robot + o_sensor; // Sum of theta_sensor and theta_robot - Eigen::Matrix2s R_sr = Eigen::Rotation2D<WolfScalar>(o_rs).matrix(); // Sum of theta_sensor and theta_robot + Scalar o_rs = o_robot + o_sensor; // Sum of theta_sensor and theta_robot + Eigen::Matrix2s R_sr = Eigen::Rotation2D<Scalar>(o_rs).matrix(); // Sum of theta_sensor and theta_robot Eigen::Vector2s p_robot_landmark = p_robot - p_landmark; // ------------------------ d @@ -415,7 +415,7 @@ Eigen::VectorXs ProcessorLaser2D::computeSquaredMahalanobisDistances(const Featu // ------------------------ Sigma_d // JACOBIAN - Eigen::Matrix<WolfScalar, 3, 6> Jlr = Eigen::Matrix<WolfScalar, 3, 6>::Zero(); + Eigen::Matrix<Scalar, 3, 6> Jlr = Eigen::Matrix<Scalar, 3, 6>::Zero(); Jlr.block<2,2>(0,0) = -R_sr.transpose(); Jlr(2,2) = -1; Jlr.block<2,2>(0,3) = R_sr.transpose(); @@ -456,7 +456,7 @@ Eigen::VectorXs ProcessorLaser2D::computeSquaredMahalanobisDistances(const Featu assert(_mu.rows() == 3 && "mahalanobis distance with bad number of mu components"); const Eigen::Vector2s& p_feature = _feature_ptr->getMeasurement().head(2); - const WolfScalar& o_feature = _feature_ptr->getMeasurement()(2); + const Scalar& o_feature = _feature_ptr->getMeasurement()(2); // ------------------------ d Eigen::Vector3s d; @@ -491,7 +491,7 @@ bool ProcessorLaser2D::fitNewContainer(FeatureCorner2D* _corner_feature_ptr, Lan && std::fabs(pi2pi(((LandmarkCorner2D*)(*landmark_it))->getAperture() + M_PI / 2)) < MAX_ACCEPTED_APERTURE_DIFF) // should be a corner { //std::cout << "landmark " << (*landmark_it)->nodeId() << std::endl; - WolfScalar SQ2 = sqrt(2)/2; + Scalar SQ2 = sqrt(2)/2; Eigen::MatrixXs corners_relative_positions(3,6); // FROM A (see LandmarkContainer.h) // FROM B // Large side // Short side // Diagonal // Large side // Short side // Diagonal @@ -499,7 +499,7 @@ bool ProcessorLaser2D::fitNewContainer(FeatureCorner2D* _corner_feature_ptr, Lan -SQ2 * CONTAINER_LENGTH, SQ2 * CONTAINER_WIDTH, SQ2 * (CONTAINER_WIDTH - CONTAINER_LENGTH), SQ2 * CONTAINER_LENGTH,-SQ2 * CONTAINER_WIDTH, SQ2 * (CONTAINER_LENGTH - CONTAINER_WIDTH), M_PI / 2, -M_PI / 2, M_PI, -M_PI / 2, M_PI / 2, M_PI; - Eigen::Matrix2s R_feature = Eigen::Rotation2D<WolfScalar>(_corner_feature_ptr->getMeasurement()(2)).matrix(); + Eigen::Matrix2s R_feature = Eigen::Rotation2D<Scalar>(_corner_feature_ptr->getMeasurement()(2)).matrix(); corners_relative_positions = - corners_relative_positions; corners_relative_positions.topRows(2) = R_feature * corners_relative_positions.topRows(2); @@ -510,7 +510,7 @@ bool ProcessorLaser2D::fitNewContainer(FeatureCorner2D* _corner_feature_ptr, Lan // for (unsigned int i = 0; i < 6; i++) // std::cout << erfc( sqrt(squared_mahalanobis_distances(i)/2) ) << std::endl; - WolfScalar SMD_threshold = 8; + Scalar SMD_threshold = 8; if (squared_mahalanobis_distances(0) < SMD_threshold ) //erfc( sqrt(squared_mahalanobis_distances(0)/2) ) > 0.8 ) { std::cout << " large side! prob = " << erfc( sqrt(squared_mahalanobis_distances(0)/2.0)) << std::endl; @@ -584,7 +584,7 @@ void ProcessorLaser2D::createCornerLandmark(FeatureCorner2D* _corner_ptr, const Sigma_robot.block<1,2>(2,0) = Sigma_robot.block<2,1>(0,2).transpose(); Eigen::Matrix3s R_robot3D = Eigen::Matrix3s::Identity(); - R_robot3D.block<2,2>(0,0) = Eigen::Rotation2D<WolfScalar>(*(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr())).matrix(); + R_robot3D.block<2,2>(0,0) = Eigen::Rotation2D<Scalar>(*(capture_laser_ptr_->getFramePtr()->getOPtr()->getPtr())).matrix(); Eigen::Matrix3s Sigma_landmark = Sigma_robot + R_robot3D.transpose() * _corner_ptr->getMeasurementCovariance().topLeftCorner<3,3>() * R_robot3D; getWolfProblem()->addCovarianceBlock(new_landmark->getPPtr(), new_landmark->getPPtr(), Sigma_landmark.topLeftCorner<2,2>()); diff --git a/src/processor_laser_2D.h b/src/processor_laser_2D.h index 65b9334d6a076a15b7b0f73ae30881c99f6b6052..adf0465d7e597bee43332015db4f2b07549b523d 100644 --- a/src/processor_laser_2D.h +++ b/src/processor_laser_2D.h @@ -41,9 +41,9 @@ namespace wolf { //some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level -const WolfScalar MAX_ACCEPTED_APERTURE_DIFF = 20.0*M_PI/180.; //20 degrees -const WolfScalar CONTAINER_WIDTH = 2.44; -const WolfScalar CONTAINER_LENGTH = 12.20; +const Scalar MAX_ACCEPTED_APERTURE_DIFF = 20.0*M_PI/180.; //20 degrees +const Scalar CONTAINER_WIDTH = 2.44; +const Scalar CONTAINER_LENGTH = 12.20; diff --git a/src/processor_motion.h b/src/processor_motion.h index ab3fd5a59c851d2898cd5fd085f1c10fe19d979c..b5a48b770052e67aa243823dc5ed73dce79cef86 100644 --- a/src/processor_motion.h +++ b/src/processor_motion.h @@ -24,7 +24,7 @@ class ProcessorMotion : public ProcessorBase // This is the main public interface public: - ProcessorMotion(ProcessorType _tp, WolfScalar _dt, size_t _state_size, size_t _delta_size, size_t _data_size); + ProcessorMotion(ProcessorType _tp, Scalar _dt, size_t _state_size, size_t _delta_size, size_t _data_size); virtual ~ProcessorMotion(); // Instructions to the processor: @@ -118,7 +118,7 @@ class ProcessorMotion : public ProcessorBase * * However, other more complicated relations are possible. */ - virtual void data2delta(const Eigen::VectorXs& _data, const WolfScalar _dt, Eigen::VectorXs& _delta) = 0; + virtual void data2delta(const Eigen::VectorXs& _data, const Scalar _dt, Eigen::VectorXs& _delta) = 0; /** \brief composes a delta-state on top of a state * \param _x the initial state @@ -173,7 +173,7 @@ class ProcessorMotion : public ProcessorBase protected: // helpers to avoid allocation - WolfScalar dt_; ///< Time step + Scalar dt_; ///< Time step Eigen::VectorXs x_; ///< state temporary Eigen::VectorXs delta_, delta_integrated_; ///< current delta and integrated deltas Eigen::VectorXs data_; ///< current data @@ -181,7 +181,7 @@ class ProcessorMotion : public ProcessorBase }; -inline ProcessorMotion::ProcessorMotion(ProcessorType _tp, WolfScalar _dt, size_t _state_size, size_t _delta_size, size_t _data_size) : +inline ProcessorMotion::ProcessorMotion(ProcessorType _tp, Scalar _dt, size_t _state_size, size_t _delta_size, size_t _data_size) : ProcessorBase(_tp), x_size_(_state_size), delta_size_(_delta_size), data_size_(_data_size), origin_ptr_(nullptr), last_ptr_(nullptr), incoming_ptr_(nullptr), dt_(_dt), x_(_state_size), diff --git a/src/processor_odom_3D.h b/src/processor_odom_3D.h index 1b76f7edfaef8d6b76b0ebeebc6e4e03b80601df..732feebad9cc34c62a87b64831053b48e2681d9b 100644 --- a/src/processor_odom_3D.h +++ b/src/processor_odom_3D.h @@ -38,9 +38,9 @@ namespace wolf { class ProcessorOdom3d : public ProcessorMotion { public: - ProcessorOdom3d(WolfScalar _delta_t = 0); + ProcessorOdom3d(Scalar _delta_t = 0); virtual ~ProcessorOdom3d(); - virtual void data2delta(const Eigen::VectorXs& _data, const WolfScalar _dt, Eigen::VectorXs& _delta); + virtual void data2delta(const Eigen::VectorXs& _data, const Scalar _dt, Eigen::VectorXs& _delta); protected: virtual void preProcess(){} @@ -62,7 +62,7 @@ class ProcessorOdom3d : public ProcessorMotion }; -inline ProcessorOdom3d::ProcessorOdom3d(WolfScalar _delta_t) : +inline ProcessorOdom3d::ProcessorOdom3d(Scalar _delta_t) : ProcessorMotion(PRC_ODOM_3D, _delta_t, 7, 7, 6), p1_(nullptr), //, q1_(nullptr) p2_(nullptr), //, q1_(nullptr) @@ -77,7 +77,7 @@ inline ProcessorOdom3d::~ProcessorOdom3d() { } -inline void ProcessorOdom3d::data2delta(const Eigen::VectorXs& _data, const WolfScalar _dt, Eigen::VectorXs& _delta) +inline void ProcessorOdom3d::data2delta(const Eigen::VectorXs& _data, const Scalar _dt, Eigen::VectorXs& _delta) { _delta.head(3) = _data.head(3); new (&q_out_) Eigen::Map<Eigen::Quaternions>(_delta.data() + 3); diff --git a/src/processor_preintegrated_imu.h b/src/processor_preintegrated_imu.h index e9adb24240d65263c798ffa4c06a794d13b3679e..12bebb8126f4b5d8e16b7ea4bae531def4e54b98 100644 --- a/src/processor_preintegrated_imu.h +++ b/src/processor_preintegrated_imu.h @@ -70,11 +70,11 @@ class ProcessorPreintegratedIMU : public ProcessorMotion2{ private: ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). - Eigen::Matrix<WolfScalar,9,9> preint_meas_cov_; + Eigen::Matrix<Scalar,9,9> preint_meas_cov_; ///Jacobians - Eigen::Matrix<WolfScalar,9,3> preintegrated_H_biasAcc_; - Eigen::Matrix<WolfScalar,9,3> preintegrated_H_biasOmega_; + Eigen::Matrix<Scalar,9,3> preintegrated_H_biasAcc_; + Eigen::Matrix<Scalar,9,3> preintegrated_H_biasOmega_; }; diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h index 7eabf6f3d46bb17c0ed47249051dde4264527dd4..dbda15cc0073ca0450a7546a79e67340cbbfe867 100644 --- a/src/processor_tracker_feature.h +++ b/src/processor_tracker_feature.h @@ -18,14 +18,14 @@ namespace wolf struct FeatureMatch { FeatureBase* feature_ptr_; - WolfScalar normalized_score_; + Scalar normalized_score_; FeatureMatch() : feature_ptr_(nullptr), normalized_score_(0.0) { } - FeatureMatch(FeatureBase* _last_feature_ptr, const WolfScalar& _normalized_score) : + FeatureMatch(FeatureBase* _last_feature_ptr, const Scalar& _normalized_score) : feature_ptr_(_last_feature_ptr), normalized_score_(_normalized_score) { diff --git a/src/processor_tracker_landmark.h b/src/processor_tracker_landmark.h index cb6673ff3c8ad8748d4512e0f82e13bd5285b5f9..00e7e2f47ee24f34dcf8481c9fd0d9f681492fa8 100644 --- a/src/processor_tracker_landmark.h +++ b/src/processor_tracker_landmark.h @@ -18,13 +18,13 @@ namespace wolf struct LandmarkMatch { LandmarkBase* landmark_ptr_; - WolfScalar normalized_score_; + Scalar normalized_score_; LandmarkMatch() : landmark_ptr_(nullptr), normalized_score_(0.0) { } - LandmarkMatch(LandmarkBase* _landmark_ptr, const WolfScalar& _normalized_score) : + LandmarkMatch(LandmarkBase* _landmark_ptr, const Scalar& _normalized_score) : landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score) { diff --git a/src/processor_tracker_laser.cpp b/src/processor_tracker_laser.cpp index 32bb14a1febcf767140661410ed632fc5b8569ce..36453e2f4e89a41b8d122660b900a39f14d8fdcf 100644 --- a/src/processor_tracker_laser.cpp +++ b/src/processor_tracker_laser.cpp @@ -38,7 +38,7 @@ unsigned int ProcessorTrackerLaser::findLandmarks(LandmarkBaseList& _landmark_li if (!new_features_incoming_.empty()) { //local declarations - WolfScalar prob, dm2; + Scalar prob, dm2; unsigned int ii, jj; // COMPUTING ALL EXPECTED FEATURES std::map<LandmarkBase*, Eigen::Vector4s> expected_features; @@ -140,11 +140,11 @@ void ProcessorTrackerLaser::extractCorners(const CaptureLaser2D* _capture_laser_ measurement(2) = corner.orientation_; measurement(3) = corner.aperture_; // TODO: maybe in line object? - WolfScalar L1 = corner.line_1_.length(); - WolfScalar L2 = corner.line_2_.length(); - WolfScalar cov_angle_line1 = 12 * corner.line_1_.error_ + Scalar L1 = corner.line_1_.length(); + Scalar L2 = corner.line_2_.length(); + Scalar cov_angle_line1 = 12 * corner.line_1_.error_ / (pow(L1, 2) * (pow(corner.line_1_.np_, 3) - pow(corner.line_1_.np_, 2))); - WolfScalar cov_angle_line2 = 12 * corner.line_2_.error_ + Scalar cov_angle_line2 = 12 * corner.line_2_.error_ / (pow(L2, 2) * (pow(corner.line_2_.np_, 3) - pow(corner.line_2_.np_, 2))); //init cov in corner coordinates measurement_cov << corner.line_1_.error_ + cov_angle_line1 * L1 * L1 / 4, 0, 0, 0, 0, corner.line_2_.error_ @@ -190,7 +190,7 @@ void ProcessorTrackerLaser::expectedFeature(LandmarkBase* _landmark_ptr, Eigen:: { // Jacobian Eigen::Vector2s p_robot_landmark = t_world_robot_.head(2) - _landmark_ptr->getPPtr()->getVector(); - Eigen::Matrix<WolfScalar, 3, 6> Jlr = Eigen::Matrix<WolfScalar, 3, 6>::Zero(); + Eigen::Matrix<Scalar, 3, 6> Jlr = Eigen::Matrix<Scalar, 3, 6>::Zero(); Jlr.block<3, 3>(0, 3) = -R_world_sensor_.transpose(); Jlr.block<3, 3>(0, 3) = R_world_sensor_.transpose(); Jlr(0, 2) = -p_robot_landmark(0) * sin(t_world_sensor_(2)) + p_robot_landmark(1) * cos(t_world_sensor_(2)); @@ -210,7 +210,7 @@ Eigen::VectorXs ProcessorTrackerLaser::computeSquaredMahalanobisDistances(const { const Eigen::Vector2s& p_feature = _feature_ptr->getMeasurement().head(2); - const WolfScalar& o_feature = _feature_ptr->getMeasurement()(2); + const Scalar& o_feature = _feature_ptr->getMeasurement()(2); // ------------------------ d Eigen::Vector3s d; d.head(2) = p_feature - _expected_feature.head(2); diff --git a/src/processor_tracker_laser.h b/src/processor_tracker_laser.h index 3d21636b6d4d46ee4f5549018a2bf5d0d5792b81..5ef0cfb44c1a5cd6b183bb580ed96ec0458c6ff9 100644 --- a/src/processor_tracker_laser.h +++ b/src/processor_tracker_laser.h @@ -26,10 +26,10 @@ namespace wolf { //some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level -const WolfScalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees -const WolfScalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees; -const WolfScalar position_error_th_ = 1; -const WolfScalar min_features_ratio_th_ = 0.5; +const Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees +const Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees; +const Scalar position_error_th_ = 1; +const Scalar min_features_ratio_th_ = 0.5; class ProcessorTrackerLaser : public ProcessorTrackerLandmark { diff --git a/src/sensor_gps_fix.cpp b/src/sensor_gps_fix.cpp index 0b13f59c413e29fb0daf1b68dc43b8f855906ec4..364729513c3126d50187bd8d22272402fedd4af2 100644 --- a/src/sensor_gps_fix.cpp +++ b/src/sensor_gps_fix.cpp @@ -13,7 +13,7 @@ SensorGPSFix::~SensorGPSFix() // } -WolfScalar SensorGPSFix::getNoise() const +Scalar SensorGPSFix::getNoise() const { return noise_std_(0); } diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp index 2e6f2aed4a90d682509fe13e7d3012b1654214f1..6f1e0472f64e8ee63f3f8e19c316c06f02ecda4e 100644 --- a/src/sensor_laser_2D.cpp +++ b/src/sensor_laser_2D.cpp @@ -2,9 +2,9 @@ namespace wolf { -// SensorLaser2D::SensorLaser2D(const Eigen::VectorXs & _sp, WolfScalar _angle_min, WolfScalar _angle_max, WolfScalar _angle_increment, WolfScalar _range_min, WolfScalar _range_max, WolfScalar _range_stdev, WolfScalar _time_increment, WolfScalar _scan_time) : +// SensorLaser2D::SensorLaser2D(const Eigen::VectorXs & _sp, Scalar _angle_min, Scalar _angle_max, Scalar _angle_increment, Scalar _range_min, Scalar _range_max, Scalar _range_stdev, Scalar _time_increment, Scalar _scan_time) : // SensorBase(LIDAR, _sp, 8) -// // SensorBase(LIDAR, _sp,{(WolfScalar)(_nrays), _apert, _rmin, _rmax, _range_stdev}) +// // SensorBase(LIDAR, _sp,{(Scalar)(_nrays), _apert, _rmin, _rmax, _range_stdev}) // { // params_ << _angle_min, _angle_max, _angle_increment, _range_min, _range_max, _range_stdev, _time_increment, _scan_time; // } @@ -89,42 +89,42 @@ const laserscanutils::ExtractCornerParams & SensorLaser2D::getCornerAlgParams() return corners_alg_params_; } -/*WolfScalar SensorLaser2D::getAngleMin() const +/*Scalar SensorLaser2D::getAngleMin() const { return params_(0); } -WolfScalar SensorLaser2D::getAngleMax() const +Scalar SensorLaser2D::getAngleMax() const { return params_(1); } -WolfScalar SensorLaser2D::getAngleIncrement() const +Scalar SensorLaser2D::getAngleIncrement() const { return params_(2); } -WolfScalar SensorLaser2D::getRangeMin() const +Scalar SensorLaser2D::getRangeMin() const { return params_(3); } -WolfScalar SensorLaser2D::getRangeMax() const +Scalar SensorLaser2D::getRangeMax() const { return params_(4); } -WolfScalar SensorLaser2D::getRangeStdDev() const +Scalar SensorLaser2D::getRangeStdDev() const { return params_(5); } -WolfScalar SensorLaser2D::getTimeIncrement() const +Scalar SensorLaser2D::getTimeIncrement() const { return params_(6); } -WolfScalar SensorLaser2D::getScanTime() const +Scalar SensorLaser2D::getScanTime() const { return params_(7); }*/ diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h index 75e9c1f6cffe34edc6971c8158678c18257a41a9..9b515ef2b68a676ed2cb81c110491c707262234e 100644 --- a/src/sensor_laser_2D.h +++ b/src/sensor_laser_2D.h @@ -74,56 +74,56 @@ class SensorLaser2D : public SensorBase * Returns angle_min * **/ -// WolfScalar getAngleMin() const; +// Scalar getAngleMin() const; /** \brief Returns angle_max * * Returns angle_max * **/ -// WolfScalar getAngleMax() const; +// Scalar getAngleMax() const; /** \brief Returns angle_increment * * Returns angle_increment * **/ -// WolfScalar getAngleIncrement() const; +// Scalar getAngleIncrement() const; /** \brief Returns range_min * * Returns range_min * **/ -// WolfScalar getRangeMin() const; +// Scalar getRangeMin() const; /** \brief Returns range_max * * Returns range_max * **/ -// WolfScalar getRangeMax() const; +// Scalar getRangeMax() const; /** \brief Returns _range_stdev * * Returns _range_stdev * **/ -// WolfScalar getRangeStdDev() const; +// Scalar getRangeStdDev() const; /** \brief Returns time_increment * * Returns time_increment * **/ -// WolfScalar getTimeIncrement() const; +// Scalar getTimeIncrement() const; /** \brief Returns scan_time * * Returns scan_time * **/ -// WolfScalar getScanTime() const; +// Scalar getScanTime() const; /** \brief Prints parameters **/ diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp index a6560c34ea7ccbd9b23278a2dc409e2773ee707a..9fefe16d7f4d1aa1dd89ed99d6af315e9a2fef99 100644 --- a/src/sensor_odom_2D.cpp +++ b/src/sensor_odom_2D.cpp @@ -2,7 +2,7 @@ namespace wolf { -SensorOdom2D::SensorOdom2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) : +SensorOdom2D::SensorOdom2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _disp_noise_factor, const Scalar& _rot_noise_factor) : SensorBase(SEN_ODOM_2D, _p_ptr, _o_ptr, nullptr, 2), k_disp_to_disp_(_disp_noise_factor), k_rot_to_rot_(_rot_noise_factor) { } @@ -12,12 +12,12 @@ SensorOdom2D::~SensorOdom2D() // } -WolfScalar SensorOdom2D::getDispVarToDispNoiseFactor() const +Scalar SensorOdom2D::getDispVarToDispNoiseFactor() const { return k_disp_to_disp_; } -WolfScalar SensorOdom2D::getRotVarToRotNoiseFactor() const +Scalar SensorOdom2D::getRotVarToRotNoiseFactor() const { return k_rot_to_rot_; } diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h index 9edb5150b4a4d65b968eb5c3436a1a0426f17806..2f1e1d47ef7c0909443c533f817f7d52f72e3be7 100644 --- a/src/sensor_odom_2D.h +++ b/src/sensor_odom_2D.h @@ -11,8 +11,8 @@ class SensorOdom2D : public SensorBase { protected: - WolfScalar k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation - WolfScalar k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation + Scalar k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation + Scalar k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation public: /** \brief Constructor with arguments @@ -24,7 +24,7 @@ class SensorOdom2D : public SensorBase * \param _rot_noise_factor rotation noise factor * **/ - SensorOdom2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor); + SensorOdom2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const Scalar& _disp_noise_factor, const Scalar& _rot_noise_factor); /** \brief Default destructor (not recommended) * diff --git a/src/solver/cost_function_sparse_base.h b/src/solver/cost_function_sparse_base.h index af0072e323e4920ff6982abf09a44ef18c55fe62..03b7b605380c015234758d8a9907206c7d50a9b5 100644 --- a/src/solver/cost_function_sparse_base.h +++ b/src/solver/cost_function_sparse_base.h @@ -32,7 +32,7 @@ template <class ConstraintT, unsigned int BLOCK_9_SIZE = 0> class CostFunctionSparseBase : CostFunctionBase { - typedef ceres::Jet<WolfScalar, BLOCK_0_SIZE + + typedef ceres::Jet<Scalar, BLOCK_0_SIZE + BLOCK_1_SIZE + BLOCK_2_SIZE + BLOCK_3_SIZE + diff --git a/src/state_block.h b/src/state_block.h index 730c5814cd7753e09c85cbf4726445617ff466ef..eabbe035b2c634773f10af10e8f00d4a649985ea 100644 --- a/src/state_block.h +++ b/src/state_block.h @@ -53,7 +53,7 @@ class StateBlock /** \brief Returns the pointer to the first element of the state **/ - WolfScalar* getPtr(); + Scalar* getPtr(); /** \brief Returns the state vector **/ @@ -102,7 +102,7 @@ inline StateBlock::~StateBlock() // } -inline WolfScalar* StateBlock::getPtr() +inline Scalar* StateBlock::getPtr() { return state_.data(); } diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp index 82b57ef152452825cd1c25804b64b0216e793fec..3d4bbef869717f8d20a336011e59128bce2b50af 100644 --- a/src/time_stamp.cpp +++ b/src/time_stamp.cpp @@ -9,14 +9,14 @@ TimeStamp::TimeStamp() : setToNow(); } -TimeStamp::TimeStamp(const WolfScalar _ts) : +TimeStamp::TimeStamp(const Scalar _ts) : time_stamp_(_ts) { // } TimeStamp::TimeStamp(const unsigned long int _sec, const unsigned long int _nsec) : - time_stamp_((WolfScalar)_sec + (WolfScalar)_nsec/(WolfScalar)1e9) + time_stamp_((Scalar)_sec + (Scalar)_nsec/(Scalar)1e9) { // } diff --git a/src/time_stamp.h b/src/time_stamp.h index ccadd40fbe7d2bc0e5c1ded187797e825718441a..9e937314552253235e41ddf1a35f9fa6c83750b5 100644 --- a/src/time_stamp.h +++ b/src/time_stamp.h @@ -22,7 +22,7 @@ namespace wolf { class TimeStamp { private: - WolfScalar time_stamp_; ///< Time stamp. Expressed in seconds from 1th jan 1970. + Scalar time_stamp_; ///< Time stamp. Expressed in seconds from 1th jan 1970. static const unsigned int TIME_STAMP_DIGITS_ = 10; ///< Number of digits to print time stamp values public: @@ -38,7 +38,7 @@ class TimeStamp * Constructor with arguments * */ - TimeStamp(const WolfScalar _ts); + TimeStamp(const Scalar _ts); /** \brief Constructor from sec and nsec * @@ -76,14 +76,14 @@ class TimeStamp * Sets time stamp to a given value passed as a scalar_t (seconds) * */ - void set(const WolfScalar ts); + void set(const Scalar ts); /** \brief Get time stamp * * Returns time stamp * */ - WolfScalar get() const; + Scalar get() const; /** \brief Get time stamp (only seconds) * @@ -104,7 +104,7 @@ class TimeStamp * Assignement operator * */ - void operator =(const WolfScalar& ts); + void operator =(const Scalar& ts); /** \brief Assignement operator * @@ -132,15 +132,15 @@ class TimeStamp * difference operator * */ - WolfScalar operator -(const TimeStamp& ts) const; + Scalar operator -(const TimeStamp& ts) const; /** \brief Add-assign operator */ - void operator +=(const WolfScalar& dt); + void operator +=(const Scalar& dt); /** \brief Add-assign operator */ - TimeStamp operator +(const WolfScalar& dt); + TimeStamp operator +(const Scalar& dt); /** \brief Prints time stamp to a given ostream * @@ -155,25 +155,25 @@ inline void TimeStamp::setToNow() { timeval ts; gettimeofday(&ts, NULL); - time_stamp_ = (WolfScalar)(ts.tv_sec) + (WolfScalar)(ts.tv_usec) / 1e6; + time_stamp_ = (Scalar)(ts.tv_sec) + (Scalar)(ts.tv_usec) / 1e6; } -inline void TimeStamp::set(const WolfScalar ts) +inline void TimeStamp::set(const Scalar ts) { time_stamp_ = ts; } inline void TimeStamp::set(const unsigned long int sec, const unsigned long int nanosec) { - time_stamp_ = (WolfScalar)(sec) + (WolfScalar)(nanosec) / (WolfScalar)(1e9); + time_stamp_ = (Scalar)(sec) + (Scalar)(nanosec) / (Scalar)(1e9); } inline void TimeStamp::set(const timeval& ts) { - time_stamp_ = (WolfScalar)(ts.tv_sec) + (WolfScalar)(ts.tv_usec) / 1e6; + time_stamp_ = (Scalar)(ts.tv_sec) + (Scalar)(ts.tv_usec) / 1e6; } -inline WolfScalar TimeStamp::get() const +inline Scalar TimeStamp::get() const { return time_stamp_; } @@ -187,8 +187,8 @@ inline unsigned long int TimeStamp::getSeconds() const inline unsigned long int TimeStamp::getNanoSeconds() const { - WolfScalar ts; - ts = (WolfScalar)((floor(time_stamp_))); + Scalar ts; + ts = (Scalar)((floor(time_stamp_))); return (unsigned long int)((time_stamp_ - ts) * 1e9); } @@ -197,7 +197,7 @@ inline void TimeStamp::operator =(const TimeStamp& ts) time_stamp_ = ts.get(); } -inline void TimeStamp::operator =(const WolfScalar& ts) +inline void TimeStamp::operator =(const Scalar& ts) { time_stamp_ = ts; } @@ -218,17 +218,17 @@ inline bool TimeStamp::operator <=(const TimeStamp& ts) const return false; } -inline WolfScalar TimeStamp::operator -(const TimeStamp& ts) const +inline Scalar TimeStamp::operator -(const TimeStamp& ts) const { return (time_stamp_ - ts.get()); } -inline void TimeStamp::operator +=(const WolfScalar& dt) +inline void TimeStamp::operator +=(const Scalar& dt) { time_stamp_ += dt; } -inline TimeStamp TimeStamp::operator +(const WolfScalar& dt) +inline TimeStamp TimeStamp::operator +(const Scalar& dt) { return TimeStamp(time_stamp_ + dt); } diff --git a/src/wolf.h b/src/wolf.h index e0b381574040c925d4f321f065b4a08cdfc33121..da526dd586864dcf44d0a5606fa42e474fdd9597 100644 --- a/src/wolf.h +++ b/src/wolf.h @@ -33,9 +33,9 @@ namespace wolf { * * The ONLY exception to this rule is when you need special precision. The ONLY example by now is the time stamp which uses double. */ -//typedef float WolfScalar; // Use this for float, 32 bit precision -typedef double WolfScalar; // Use this for double, 64 bit precision -//typedef long double WolfScalar; // Use this for long double, 128 bit precision +//typedef float Scalar; // Use this for float, 32 bit precision +typedef double Scalar; // Use this for double, 64 bit precision +//typedef long double Scalar; // Use this for long double, 128 bit precision namespace Constants{ @@ -52,7 +52,7 @@ const double MIN_VARIANCE = 1e-6; } // namespace wolf /////////////////////////////////////////// -// Construct types for any scalar defined in the typedef WolfScalar above +// Construct types for any scalar defined in the typedef Scalar above //////////////////////////////////////////// /** \brief Namespace extending Eigen definitions * @@ -60,34 +60,34 @@ const double MIN_VARIANCE = 1e-6; * The appended letter indicating this is 's', so that we have, e.g., * - VectorXf Vector of floats - defined by Eigen * - VectorXd Vector of doubles - defined by Eigen - * - VectorXs Vector of either double of float, depending on the type \b WolfScalar, defined by Wolf. + * - VectorXs Vector of either double of float, depending on the type \b Scalar, defined by Wolf. * */ namespace Eigen // Eigen namespace extension { // 1. Vectors and Matrices -typedef Matrix<wolf::WolfScalar, 2, 2, Eigen::RowMajor> Matrix2s; ///< 2x2 matrix of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 3, 3, Eigen::RowMajor> Matrix3s; ///< 3x3 matrix of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 4, 4, Eigen::RowMajor> Matrix4s; ///< 4x4 matrix of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 7, 7, Eigen::RowMajor> Matrix7s; ///< 7x7 matrix of real WolfScalar type -typedef Matrix<wolf::WolfScalar, Dynamic, Dynamic, Eigen::RowMajor> MatrixXs; ///< variable size matrix of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 1, 1> Vector1s; ///< 1-vector of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 2, 1> Vector2s; ///< 2-vector of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 3, 1> Vector3s; ///< 3-vector of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 4, 1> Vector4s; ///< 4-vector of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 6, 1> Vector6s; ///< 6-vector of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 7, 1> Vector7s; ///< 7-vector of real WolfScalar type -typedef Matrix<wolf::WolfScalar, Dynamic, 1> VectorXs; ///< variable size vector of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 1, 2> RowVector2s; ///< 2-row-vector of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 1, 3> RowVector3s; ///< 3-row-vector of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 1, 4> RowVector4s; ///< 4-row-vector of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 1, 7> RowVector7s; ///< 7-row-vector of real WolfScalar type -typedef Matrix<wolf::WolfScalar, 1, Dynamic> RowVectorXs; ///< variable size row-vector of real WolfScalar type +typedef Matrix<wolf::Scalar, 2, 2, Eigen::RowMajor> Matrix2s; ///< 2x2 matrix of real Scalar type +typedef Matrix<wolf::Scalar, 3, 3, Eigen::RowMajor> Matrix3s; ///< 3x3 matrix of real Scalar type +typedef Matrix<wolf::Scalar, 4, 4, Eigen::RowMajor> Matrix4s; ///< 4x4 matrix of real Scalar type +typedef Matrix<wolf::Scalar, 7, 7, Eigen::RowMajor> Matrix7s; ///< 7x7 matrix of real Scalar type +typedef Matrix<wolf::Scalar, Dynamic, Dynamic, Eigen::RowMajor> MatrixXs; ///< variable size matrix of real Scalar type +typedef Matrix<wolf::Scalar, 1, 1> Vector1s; ///< 1-vector of real Scalar type +typedef Matrix<wolf::Scalar, 2, 1> Vector2s; ///< 2-vector of real Scalar type +typedef Matrix<wolf::Scalar, 3, 1> Vector3s; ///< 3-vector of real Scalar type +typedef Matrix<wolf::Scalar, 4, 1> Vector4s; ///< 4-vector of real Scalar type +typedef Matrix<wolf::Scalar, 6, 1> Vector6s; ///< 6-vector of real Scalar type +typedef Matrix<wolf::Scalar, 7, 1> Vector7s; ///< 7-vector of real Scalar type +typedef Matrix<wolf::Scalar, Dynamic, 1> VectorXs; ///< variable size vector of real Scalar type +typedef Matrix<wolf::Scalar, 1, 2> RowVector2s; ///< 2-row-vector of real Scalar type +typedef Matrix<wolf::Scalar, 1, 3> RowVector3s; ///< 3-row-vector of real Scalar type +typedef Matrix<wolf::Scalar, 1, 4> RowVector4s; ///< 4-row-vector of real Scalar type +typedef Matrix<wolf::Scalar, 1, 7> RowVector7s; ///< 7-row-vector of real Scalar type +typedef Matrix<wolf::Scalar, 1, Dynamic> RowVectorXs; ///< variable size row-vector of real Scalar type // 2. Quaternions and other rotation things -typedef Quaternion<wolf::WolfScalar> Quaternions; ///< Quaternion of real WolfScalar type -typedef AngleAxis<wolf::WolfScalar> AngleAxiss; ///< Angle-Axis of real WolfScalar type -typedef Rotation2D<wolf::WolfScalar> Rotation2Ds; ///< Rotation2D of real WolfScalar type +typedef Quaternion<wolf::Scalar> Quaternions; ///< Quaternion of real Scalar type +typedef AngleAxis<wolf::Scalar> AngleAxiss; ///< Angle-Axis of real Scalar type +typedef Rotation2D<wolf::Scalar> Rotation2Ds; ///< Rotation2D of real Scalar type } namespace wolf { @@ -337,7 +337,7 @@ typedef std::list<StateBlock*> StateBlockList; typedef StateBlockList::iterator StateBlockIter; -inline WolfScalar pi2pi(const WolfScalar& angle) +inline Scalar pi2pi(const Scalar& angle) { return (angle > 0 ? fmod(angle + M_PI, 2 * M_PI) - M_PI : fmod(angle - M_PI, 2 * M_PI) + M_PI); } @@ -347,7 +347,7 @@ inline WolfScalar pi2pi(const WolfScalar& angle) // Quaternion things namespace Eigen{ inline void v2q(const Eigen::VectorXs& _v, Eigen::Quaternions& _q){ - wolf::WolfScalar angle = _v.norm(); + wolf::Scalar angle = _v.norm(); if (angle < wolf::Constants::EPS) _q = Eigen::Quaternions::Identity(); else @@ -357,7 +357,7 @@ inline void v2q(const Eigen::VectorXs& _v, Eigen::Quaternions& _q){ } inline void v2q(const Eigen::VectorXs& _v, Eigen::Map<Eigen::Quaternions>& _q){ - wolf::WolfScalar angle = _v.norm(); + wolf::Scalar angle = _v.norm(); if (angle < wolf::Constants::EPS) _q = Eigen::Quaternions::Identity(); else diff --git a/src/wolf_manager.cpp b/src/wolf_manager.cpp index 94d87b8d64004a889a15e877526b4378cb427964..620a33a2a3f39dff8b8c5da2fbefbf4667e7aaf6 100644 --- a/src/wolf_manager.cpp +++ b/src/wolf_manager.cpp @@ -8,7 +8,7 @@ WolfManager::WolfManager(const FrameStructure _frame_structure, const Eigen::VectorXs& _prior, const Eigen::MatrixXs& _prior_cov, const unsigned int& _trajectory_size, - const WolfScalar& _new_frame_elapsed_time) : + const Scalar& _new_frame_elapsed_time) : problem_(new WolfProblem(_frame_structure)), sensor_prior_(_sensor_prior_ptr), current_frame_(nullptr), @@ -230,7 +230,7 @@ void WolfManager::setWindowSize(const unsigned int& _size) } -void WolfManager::setNewFrameElapsedTime(const WolfScalar& _dt) +void WolfManager::setNewFrameElapsedTime(const Scalar& _dt) { new_frame_elapsed_time_ = _dt; } diff --git a/src/wolf_manager.h b/src/wolf_manager.h index 7b1c67030892e16446315da2bd05fed48ae09079..e3f852d51bc6da8289171df875d29a7988dc7e93 100644 --- a/src/wolf_manager.h +++ b/src/wolf_manager.h @@ -55,7 +55,7 @@ class WolfManager //Manager parameters unsigned int trajectory_size_; - WolfScalar new_frame_elapsed_time_; + Scalar new_frame_elapsed_time_; public: WolfManager(const FrameStructure _frame_structure, @@ -63,7 +63,7 @@ class WolfManager const Eigen::VectorXs& _prior, const Eigen::MatrixXs& _prior_cov, const unsigned int& _trajectory_size = 10, - const WolfScalar& _new_frame_elapsed_time = 0.1); + const Scalar& _new_frame_elapsed_time = 0.1); virtual ~WolfManager(); @@ -87,7 +87,7 @@ class WolfManager void setWindowSize(const unsigned int& _size); - void setNewFrameElapsedTime(const WolfScalar& _dt); + void setNewFrameElapsedTime(const Scalar& _dt); }; } // namespace wolf diff --git a/src/wolf_problem.h b/src/wolf_problem.h index 0c360423028839a44ead3aeec37ebadac2917711..09e75b2fd7c83c0f9a5c8471838d662263654341 100644 --- a/src/wolf_problem.h +++ b/src/wolf_problem.h @@ -45,7 +45,7 @@ class WolfProblem : public NodeBase StateBlockList state_block_ptr_list_; std::list<StateBlock*> state_block_add_list_; std::list<StateBlock*> state_block_update_list_; - std::list<WolfScalar*> state_block_remove_list_; + std::list<Scalar*> state_block_remove_list_; std::list<ConstraintBase*> constraint_add_list_; std::list<unsigned int> constraint_remove_list_; @@ -197,7 +197,7 @@ class WolfProblem : public NodeBase /** \brief Gets a queue of state blocks to be removed from the solver */ - std::list<WolfScalar*>* getStateBlockRemoveList(); + std::list<Scalar*>* getStateBlockRemoveList(); /** \brief Gets a queue of constraint ids to be added in the solver */ @@ -230,7 +230,7 @@ class WolfProblem : public NodeBase namespace wolf { -inline std::list<WolfScalar*>* WolfProblem::getStateBlockRemoveList() +inline std::list<Scalar*>* WolfProblem::getStateBlockRemoveList() { return &state_block_remove_list_; }