diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 2a49b96da7b7fa4df0d7beb7296d944398bc4366..b7ddaffe9d694e6ba363936522998ff22fb9775f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -92,6 +92,7 @@ SET(HDRS node_linked.h processor_base.h processor_laser_2D.h + raw_data_satellite.h sensor_base.h sensor_odom_2D.h sensor_gps_fix.h diff --git a/src/capture_fix.cpp b/src/capture_fix.cpp index d665f7aebe1d7a77914c03effdab28450e3793e4..5a413a1e2f080e7cc81d34facd2c56ce3d9845ba 100644 --- a/src/capture_fix.cpp +++ b/src/capture_fix.cpp @@ -6,7 +6,7 @@ CaptureFix::CaptureFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eige data_(_data), data_covariance_(_data_covariance) { - std::cout << "capture fix constructor " << std::endl; + //std::cout << "capture fix constructor " << std::endl; } CaptureFix::~CaptureFix() diff --git a/src/capture_odom_2D.h b/src/capture_odom_2D.h index ad535c270a7988e780cd3c0de743f2c310a3b4fd..b8c996418d6c81e1d7e987316cd66c3f9100762e 100644 --- a/src/capture_odom_2D.h +++ b/src/capture_odom_2D.h @@ -15,9 +15,8 @@ class CaptureOdom2D : public CaptureMotion { protected: - Eigen::VectorXs data_; ///< Raw data. - Eigen::MatrixXs data_covariance_; ///< Noise of the capture. - + //Eigen::VectorXs data_; ///< Raw data. + //Eigen::MatrixXs data_covariance_; ///< Noise of the capture. public: diff --git a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h b/src/ceres_wrapper/auto_diff_cost_function_wrapper.h index 978da76a800479b2c588386e00387f079c4f3609..f948cf58ed05dcba062afa286c107bece2e14976 100644 --- a/src/ceres_wrapper/auto_diff_cost_function_wrapper.h +++ b/src/ceres_wrapper/auto_diff_cost_function_wrapper.h @@ -55,7 +55,7 @@ class AutoDiffCostFunctionWrapper : public ceres::SizedCostFunction<MEASUREMENT_ virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // only residuals - if (jacobians != nullptr) + if (jacobians == nullptr) { (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4], parameters[5], parameters[6], parameters[7], parameters[8], parameters[9], residuals); @@ -107,13 +107,12 @@ class AutoDiffCostFunctionWrapper : public ceres::SizedCostFunction<MEASUREMENT_ residuals[i] = residuals_jets[i].a; // fill the jacobian matrices - if (jacobians != nullptr) - for (i = 0; i<this->n_blocks_; i++) - if (jacobians[i] != nullptr) - for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) - std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), - residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), - jacobians[i] + row * block_sizes_.at(i)); + for (i = 0; i<this->n_blocks_; i++) + if (jacobians[i] != nullptr) + for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) + std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), + residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), + jacobians[i] + row * block_sizes_.at(i)); } return true; } @@ -169,7 +168,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // only residuals - if (jacobians != nullptr) + if (jacobians == nullptr) (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4], parameters[5], parameters[6], parameters[7], parameters[8], residuals); @@ -217,13 +216,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, residuals[i] = residuals_jets[i].a; // fill the jacobian matrices - if (jacobians != nullptr) - for (i = 0; i<n_blocks_; i++) - if (jacobians[i] != nullptr) - for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) - std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), - residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), - jacobians[i] + row * block_sizes_.at(i)); + for (i = 0; i<n_blocks_; i++) + if (jacobians[i] != nullptr) + for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) + std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), + residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), + jacobians[i] + row * block_sizes_.at(i)); } return true; } @@ -278,7 +276,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // only residuals - if (jacobians != nullptr) + if (jacobians == nullptr) (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4], parameters[5], parameters[6], parameters[7], residuals); @@ -323,13 +321,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, residuals[i] = residuals_jets[i].a; // fill the jacobian matrices - if (jacobians != nullptr) - for (i = 0; i<n_blocks_; i++) - if (jacobians[i] != nullptr) - for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) - std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), - residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), - jacobians[i] + row * block_sizes_.at(i)); + for (i = 0; i<n_blocks_; i++) + if (jacobians[i] != nullptr) + for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) + std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), + residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), + jacobians[i] + row * block_sizes_.at(i)); } return true; } @@ -383,7 +380,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // only residuals - if (jacobians != nullptr) + if (jacobians == nullptr) (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4], parameters[5], parameters[6], residuals); @@ -425,13 +422,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, residuals[i] = residuals_jets[i].a; // fill the jacobian matrices - if (jacobians != nullptr) - for (i = 0; i<n_blocks_; i++) - if (jacobians[i] != nullptr) - for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) - std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), - residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), - jacobians[i] + row * block_sizes_.at(i)); + for (i = 0; i<n_blocks_; i++) + if (jacobians[i] != nullptr) + for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) + std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), + residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), + jacobians[i] + row * block_sizes_.at(i)); } return true; } @@ -484,7 +480,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // only residuals - if (jacobians != nullptr) + if (jacobians == nullptr) (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4], parameters[5], residuals); @@ -523,13 +519,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, residuals[i] = residuals_jets[i].a; // fill the jacobian matrices - if (jacobians != nullptr) - for (i = 0; i<n_blocks_; i++) - if (jacobians[i] != nullptr) - for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) - std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), - residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), - jacobians[i] + row * block_sizes_.at(i)); + for (i = 0; i<n_blocks_; i++) + if (jacobians[i] != nullptr) + for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) + std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), + residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), + jacobians[i] + row * block_sizes_.at(i)); } return true; } @@ -578,7 +573,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // only residuals - if (jacobians != nullptr) + if (jacobians == nullptr) (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], parameters[4], residuals); @@ -614,13 +609,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, residuals[i] = residuals_jets[i].a; // fill the jacobian matrices - if (jacobians != nullptr) - for (i = 0; i<n_blocks_; i++) - if (jacobians[i] != nullptr) - for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) - std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), - residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), - jacobians[i] + row * block_sizes_.at(i)); + for (i = 0; i<n_blocks_; i++) + if (jacobians[i] != nullptr) + for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) + std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), + residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), + jacobians[i] + row * block_sizes_.at(i)); } return true; } @@ -668,7 +662,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // only residuals - if (jacobians != nullptr) + if (jacobians == nullptr) (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], parameters[3], residuals); // also compute jacobians else @@ -699,13 +693,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, residuals[i] = residuals_jets[i].a; // fill the jacobian matrices - if (jacobians != nullptr) - for (i = 0; i<n_blocks_; i++) - if (jacobians[i] != nullptr) - for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) - std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), - residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), - jacobians[i] + row * block_sizes_.at(i)); + for (i = 0; i<n_blocks_; i++) + if (jacobians[i] != nullptr) + for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) + std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), + residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), + jacobians[i] + row * block_sizes_.at(i)); } return true; } @@ -752,7 +745,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // only residuals - if (jacobians != nullptr) + if (jacobians == nullptr) (*this->constraint_ptr_)(parameters[0], parameters[1], parameters[2], residuals); // also compute jacobians @@ -781,13 +774,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, residuals[i] = residuals_jets[i].a; // fill the jacobian matrices - if (jacobians != nullptr) - for (i = 0; i<n_blocks_; i++) - if (jacobians[i] != nullptr) - for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) - std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), - residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), - jacobians[i] + row * block_sizes_.at(i)); + for (i = 0; i<n_blocks_; i++) + if (jacobians[i] != nullptr) + for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) + std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), + residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), + jacobians[i] + row * block_sizes_.at(i)); } return true; } @@ -833,7 +825,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // only residuals - if (jacobians != nullptr) + if (jacobians == nullptr) (*this->constraint_ptr_)(parameters[0], parameters[1], residuals); // also compute jacobians @@ -859,13 +851,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, residuals[i] = residuals_jets[i].a; // fill the jacobian matrices - if (jacobians != nullptr) - for (i = 0; i<n_blocks_; i++) - if (jacobians[i] != nullptr) - for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) - std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), - residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), - jacobians[i] + row * block_sizes_.at(i)); + for (i = 0; i<n_blocks_; i++) + if (jacobians[i] != nullptr) + for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) + std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), + residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), + jacobians[i] + row * block_sizes_.at(i)); } return true; } @@ -910,7 +901,7 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // only residuals - if (jacobians != nullptr) + if (jacobians == nullptr) (*this->constraint_ptr_)(parameters[0], residuals); // also compute jacobians else @@ -932,13 +923,12 @@ class AutoDiffCostFunctionWrapper<ConstraintType, MEASUREMENT_SIZE, residuals[i] = residuals_jets[i].a; // fill the jacobian matrices - if (jacobians != nullptr) - for (i = 0; i<n_blocks_; i++) - if (jacobians[i] != nullptr) - for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) - std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), - residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), - jacobians[i] + row * block_sizes_.at(i)); + for (i = 0; i<n_blocks_; i++) + if (jacobians[i] != nullptr) + for (unsigned int row = 0; row < MEASUREMENT_SIZE; row++) + std::copy(residuals_jets[row].v.data() + jacobian_locations_.at(i), + residuals_jets[row].v.data() + jacobian_locations_.at(i) + block_sizes_.at(i), + jacobians[i] + row * block_sizes_.at(i)); } return true; } diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 75bd626e9e7e6856e32f17a13a074f1d7585e76a..889361768288837c37f3fa2555a4950cfb73f971 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -37,7 +37,6 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_ // run Ceres Solver ceres::Solve(_ceres_options, ceres_problem_, &ceres_summary_); - //std::cout << "solved" << std::endl; //return results return ceres_summary_; @@ -224,7 +223,7 @@ void CeresManager::removeConstraint(const unsigned int& _corr_id) void CeresManager::addStateBlock(StateBlock* _st_ptr) { - //std::cout << "Adding State Unit " << _st_ptr->nodeId() << std::endl; + //std::cout << "Adding State Unit with size: " << _st_ptr->getSize() << std::endl; //_st_ptr->print(); switch (_st_ptr->getType()) diff --git a/src/constraint_container.h b/src/constraint_container.h index dd45e22fc745ec8036400be795f8aa8c55a45813..53deffda0b552067c17f47a19a524c29ffe2ce4f 100644 --- a/src/constraint_container.h +++ b/src/constraint_container.h @@ -21,7 +21,6 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1> corner_(_corner) { assert(_corner >= 0 && _corner <= 3 && "Wrong corner id in constraint container constructor"); - lmk_ptr_->addConstraintTo(this); } /** \brief Default destructor (not recommended) @@ -32,7 +31,6 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1> virtual ~ConstraintContainer() { //std::cout << "deleting ConstraintContainer " << nodeId() << std::endl; - lmk_ptr_->removeConstraintTo(this); } LandmarkContainer* getLandmarkPtr() diff --git a/src/constraint_corner_2D.h b/src/constraint_corner_2D.h index d603f91655203a1d5faa4fae8f3fd8d28f754b95..e74217a38900fe5d01ee5c8a8ad7b1a03f9c9845 100644 --- a/src/constraint_corner_2D.h +++ b/src/constraint_corner_2D.h @@ -14,7 +14,7 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1> ConstraintCorner2D(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr, ConstraintStatus _status = CTR_ACTIVE) : ConstraintSparse<3,2,1,2,1>(_ftr_ptr, CTR_CORNER_2D, _lmk_ptr, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()) { - landmark_ptr_->addConstraintTo(this); + // } /** \brief Default destructor (not recommended) @@ -25,7 +25,6 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1> virtual ~ConstraintCorner2D() { //std::cout << "deleting ConstraintCorner2D " << nodeId() << std::endl; - landmark_ptr_->removeConstraintTo(this); } LandmarkCorner2D* getLandmarkPtr() diff --git a/src/constraint_fix.h b/src/constraint_fix.h index fef49f4b9b768cf193be59ffb4d5d5512d1eae69..6258758b88500251a3952ca6c47758650a766b45 100644 --- a/src/constraint_fix.h +++ b/src/constraint_fix.h @@ -30,6 +30,8 @@ class ConstraintFix: public ConstraintSparse<3,2,1> template <typename T> bool operator()(const T* const _p, const T* const _o, T* _residuals) const { + //std::cout << "computing constraint odom ..." << std::endl; + _residuals[0] = (T(getMeasurement()(0)) - _p[0]) / T(sqrt(getMeasurementCovariance()(0,0))); _residuals[1] = (T(getMeasurement()(1)) - _p[1]) / T(sqrt(getMeasurementCovariance()(1,1))); _residuals[2] = T(getMeasurement()(2)) - _o[0]; @@ -49,6 +51,8 @@ class ConstraintFix: public ConstraintSparse<3,2,1> // std::cout << "residual: " << _residuals[2] << std::endl << std::endl; _residuals[2] = _residuals[2] / T(sqrt(getMeasurementCovariance()(2,2))); + //std::cout << "constraint fix computed!" << std::endl; + return true; } }; diff --git a/src/constraint_odom_2D.h b/src/constraint_odom_2D.h index 312d55c3e39b8f592d3dcaa95753b70e4ee875d3..83c9706ba5bb3fb246d429205966802e521c29c3 100644 --- a/src/constraint_odom_2D.h +++ b/src/constraint_odom_2D.h @@ -29,6 +29,8 @@ class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1> template<typename T> bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const { + //std::cout << "computing constraint odom ..." << std::endl; + // std::cout << "_p1: "; // for (int i=0; i < 2; i++) // std::cout << "\n\t" << _p1[i]; @@ -72,6 +74,8 @@ class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1> _residuals[2] = _residuals[2] / T(sqrt(std::max(getMeasurementCovariance()(2, 2),1e-6))); + //std::cout << "constraint odom computed!" << std::endl; + return true; } }; diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index c637f88d73f1188d3b03d0e3d22c009d61696645..6973102865834ef7b0bfa412f6b65f20f3744c17 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -52,6 +52,11 @@ IF(faramotics_FOUND) ${pose_state_time_LIBRARIES} ${faramotics_LIBRARIES} ${PROJECT_NAME}) + ADD_EXECUTABLE(test_autodiff test_autodiff.cpp) + TARGET_LINK_LIBRARIES(test_autodiff + ${pose_state_time_LIBRARIES} + ${faramotics_LIBRARIES} + ${PROJECT_NAME}) IF(Suitesparse_FOUND) ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp) diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp index a75890a5a1079db6836b04e711a28f78372f96b2..9250a851ce250ff1b84d3fcaa313892095332ebf 100644 --- a/src/examples/solver/test_iQR_wolf2.cpp +++ b/src/examples/solver/test_iQR_wolf2.cpp @@ -19,6 +19,7 @@ //Wolf includes #include "state_block.h" #include "constraint_base.h" +#include "sensor_laser_2D.h" #include "wolf_manager.h" // wolf solver @@ -281,16 +282,16 @@ int main(int argc, char *argv[]) // DRAWING STUFF --------------------------- // draw detected corners - std::list < laserscanutils::Corner > corner_list; - std::vector<double> corner_vector; - CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1); - last_scan.extractCorners(corner_list); - for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++) - { - corner_vector.push_back(corner_it->pt_(0)); - corner_vector.push_back(corner_it->pt_(1)); - } - myRender->drawCorners(laser1Pose, corner_vector); +// std::list < laserscanutils::Corner > corner_list; +// std::vector<double> corner_vector; +// CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1); +// last_scan.extractCorners(corner_list); +// for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++) +// { +// corner_vector.push_back(corner_it->pt_(0)); +// corner_vector.push_back(corner_it->pt_(1)); +// } +// myRender->drawCorners(laser1Pose, corner_vector); // draw landmarks std::vector<double> landmark_vector; diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index c3ac998f6a90ba086b377719850179943547e9a4..e3431fd54718399ed4f0fc427a274e671d23d10e 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -17,6 +17,7 @@ //Wolf includes #include "wolf_manager.h" +#include "sensor_laser_2D.h" #include "ceres_wrapper/ceres_manager.h" //C includes for sleep, time and main args @@ -228,16 +229,16 @@ int main(int argc, char** argv) // DRAWING STUFF --------------------------- t1 = clock(); // draw detected corners - std::list < laserscanutils::Corner > corner_list; - std::vector<double> corner_vector; - CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1); - last_scan.extractCorners(corner_list); - for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++) - { - corner_vector.push_back(corner_it->pt_(0)); - corner_vector.push_back(corner_it->pt_(1)); - } - myRender->drawCorners(laser1Pose, corner_vector); +// std::list < laserscanutils::Corner > corner_list; +// std::vector<double> corner_vector; +// CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1); +// last_scan.extractCorners(corner_list); +// for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++) +// { +// corner_vector.push_back(corner_it->pt_(0)); +// corner_vector.push_back(corner_it->pt_(1)); +// } +// myRender->drawCorners(laser1Pose, corner_vector); // draw landmarks std::vector<double> landmark_vector; diff --git a/src/feature_odom_2D.cpp b/src/feature_odom_2D.cpp index 4c60e41efb87d613b93284a4283605ee841a37b4..0eb5be10782e3d1511a83cf4ecf74ec8a8bcfc18 100644 --- a/src/feature_odom_2D.cpp +++ b/src/feature_odom_2D.cpp @@ -9,7 +9,7 @@ FeatureOdom2D::FeatureOdom2D(unsigned int _dim_measurement) : FeatureOdom2D::FeatureOdom2D(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) : FeatureBase(_measurement, _meas_covariance) { - // + //std::cout << "New FeatureOdom2D: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl; } FeatureOdom2D::~FeatureOdom2D() diff --git a/src/processor_laser_2D.cpp b/src/processor_laser_2D.cpp index 0b05a15ec498341b9662f91c4491598b8550d752..4f24b920c6346e75df90bfe920a2af162068bb5f 100644 --- a/src/processor_laser_2D.cpp +++ b/src/processor_laser_2D.cpp @@ -2,7 +2,7 @@ ProcessorLaser2D::ProcessorLaser2D() : - sensor_laser_ptr_((SensorLaser2D*)(upperNodePtr())), // Static cast to specific sensor at construction time + //sensor_laser_ptr_((SensorLaser2D*)(upperNodePtr())), // Static cast to specific sensor at construction time TODO: in construction time upperNodePtr is nullptr, it crashes always, to be removed or changed to somewhere (JVN) capture_laser_ptr_(nullptr) { } @@ -13,6 +13,9 @@ ProcessorLaser2D::~ProcessorLaser2D() void ProcessorLaser2D::extractFeatures(CaptureBase* capture_ptr_) { + // TODO: Or we always cast (so pointer is not needed) or we find the place to be casted once, but constructor is not the place. + sensor_laser_ptr_ = (SensorLaser2D*)(upperNodePtr()); + //check CaptureBase pointer is the appropriate one for this Processor // assert( capture_laser_ptr_ = dynamic_cast<CaptureLaser2D*>(capture_ptr_) && "Invalid Capture Type pointer" ); capture_laser_ptr_ = (CaptureLaser2D*)(capture_ptr_); diff --git a/src/solver/qr_solver.h b/src/solver/qr_solver.h index e1f86e5ddd40e5c739b80d826ab85ed26c5bbf90..b8991339c3f17f13949173363279d645f027caca 100644 --- a/src/solver/qr_solver.h +++ b/src/solver/qr_solver.h @@ -14,7 +14,13 @@ //Wolf includes #include "state_block.h" -#include "constraint_base.h" +#include "../constraint_sparse.h" +#include "../constraint_fix.h" +#include "../constraint_gps_2D.h" +#include "../constraint_gps_pseudorange.h" +#include "../constraint_odom_2D.h" +#include "../constraint_corner_2D.h" +#include "../constraint_container.h" #include "sparse_utils.h" // wolf solver diff --git a/src/wolf_manager.cpp b/src/wolf_manager.cpp index 5fa139d0889fe97b9a97c6a6001520240166a588..2184fce633992497b9766757bf7c46969877c8b2 100644 --- a/src/wolf_manager.cpp +++ b/src/wolf_manager.cpp @@ -40,8 +40,6 @@ WolfManager::WolfManager(const FrameStructure _frame_structure, //std::cout << " initial_covariance" << std::endl; current_frame_->addCapture(initial_covariance); //std::cout << " addCapture" << std::endl; - initial_covariance->process(); - //std::cout << " processCapture" << std::endl; // Current robot frame createFrame(_prior, TimeStamp(0));