diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp index 4c51f0ce3b02fae03e81bb97db758092e64e01c0..c37b61ab339ed274777d7a1f58bbf8a2c8ee0fb2 100644 --- a/src/capture_laser_2D.cpp +++ b/src/capture_laser_2D.cpp @@ -8,18 +8,31 @@ // unsigned int CaptureLaser2D::max_beam_distance = 5;//max number of beams of distance between lines to consider corner or concatenation // double CaptureLaser2D::max_distance = 0.5;//max distance between line ends to consider corner or concatenation -CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges): - CaptureBase(_ts, _sensor_ptr, _ranges), - ranges_(data_.data(), _ranges.size()), - intensities_(data_.data(), 0) +//CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges): +// CaptureBase(_ts, _sensor_ptr, _ranges), +// ranges_(data_.data(), _ranges.size()), +// intensities_(data_.data(), 0) +//{ +// laser_ptr_ = (SensorLaser2D*)sensor_ptr_; +//} +CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges): + CaptureBase(_ts, _sensor_ptr), + ranges_(_ranges) { laser_ptr_ = (SensorLaser2D*)sensor_ptr_; } -CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities): - CaptureBase(_ts, _sensor_ptr, _ranges), - ranges_(data_.data(), _ranges.size()), - intensities_(data_.data(), _intensities.size()) +//CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities): +// CaptureBase(_ts, _sensor_ptr, _ranges), +// ranges_(data_.data(), _ranges.size()), +// intensities_(data_.data(), _intensities.size()) +//{ +// laser_ptr_ = (SensorLaser2D*)sensor_ptr_; +//} +CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges, const std::vector<float>& _intensities): + CaptureBase(_ts, _sensor_ptr), + ranges_(_ranges), + intensities_(_intensities) { laser_ptr_ = (SensorLaser2D*)sensor_ptr_; } @@ -37,15 +50,15 @@ void CaptureLaser2D::processCapture() //extract corners from range data extractCorners(corners); - //std::cout << corners.size() << " corners extracted" << std::endl; + std::cout << corners.size() << " corners extracted" << std::endl; //generate a feature for each corner createFeatures(corners); - //std::cout << getFeatureListPtr()->size() << " Features created" << std::endl; + std::cout << getFeatureListPtr()->size() << " Features created" << std::endl; //Establish constraints for each feature establishConstraints(); - //std::cout << "Constraints created" << std::endl; + std::cout << "Constraints created" << std::endl; } unsigned int CaptureLaser2D::extractCorners(std::list<laserscanutils::Corner> & _corner_list) const diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h index 8f4c537af9b9f54dd98f60a46a723086a8d5fdde..776fa0ba3bace880036c449b6e3e6d319441a69f 100644 --- a/src/capture_laser_2D.h +++ b/src/capture_laser_2D.h @@ -37,9 +37,11 @@ class CaptureLaser2D : public CaptureBase // static double k_sigmas;//How many std_dev are tolerated to count that a point is supporting a line // static unsigned int max_beam_distance;//max number of beams of distance between lines to consider corner or concatenation // static double max_distance;//max distance between line ends to consider corner or concatenation - - Eigen::Map<Eigen::VectorXs> ranges_; // a map to the ranges inside de data vector - Eigen::Map<Eigen::VectorXs> intensities_; // a map to the intensities inside the data vector + + //Eigen::Map<Eigen::VectorXs> ranges_; // a map to the ranges inside de data vector + std::vector<float> ranges_; // ranges vector + //Eigen::Map<Eigen::VectorXs> intensities_; // a map to the intensities inside the data vector + std::vector<float> intensities_; // intensities vector SensorLaser2D* laser_ptr_; //specific pointer to sensor laser 2D object public: @@ -48,14 +50,16 @@ class CaptureLaser2D : public CaptureBase * Constructor with ranges * **/ - CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges); + //CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges); + CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges); /** \brief Constructor with ranges and intensities * * Constructor with ranges and intensities * **/ - CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities); + //CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities); + CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges, const std::vector<float>& _intensities); /** \brief Destructor * diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp index 884d68c457b19a941bd9e31b14a576feb6ce7e12..5c7b366cf74c8e300bdaff56688abf2e303fbaa3 100644 --- a/src/capture_odom_2D.cpp +++ b/src/capture_odom_2D.cpp @@ -10,9 +10,9 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, cons CaptureRelative(_ts, _sensor_ptr, _data) { data_covariance_ = Eigen::Matrix2s::Zero(); - data_covariance_(0,0) = _data(0)*((SensorOdom2D*)_sensor_ptr)->getDisplacementNoiseFactor(); - data_covariance_(1,1) = _data(1)*((SensorOdom2D*)_sensor_ptr)->getRotationNoiseFactor(); - std::cout << data_covariance_ << std::endl; + data_covariance_(0,0) = (_data(0) == 0 ? 1e-6 : _data(0))*((SensorOdom2D*)_sensor_ptr)->getDisplacementNoiseFactor(); + data_covariance_(1,1) = (_data(1) == 0 ? 1e-6 : _data(1))*((SensorOdom2D*)_sensor_ptr)->getRotationNoiseFactor(); +// std::cout << data_covariance_ << std::endl; } CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) : diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index 5530fb4bd2c9cafcdd5a96eff4edd6bb2c285170..bf0a8a8cc349257129574792818b0fb3f9ca7ec3 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -198,8 +198,8 @@ int main(int argc, char** argv) ground_truth.segment(step*3,3) << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head(); // compute odometry - odom_reading(0) += distribution_odom(generator)*(odom_reading(0)+1e-3); - odom_reading(1) += distribution_odom(generator)*(odom_reading(1)+1e-3); + odom_reading(0) += distribution_odom(generator)*(odom_reading(0) == 0 ? 1e-6 : odom_reading(0)); + odom_reading(1) += distribution_odom(generator)*(odom_reading(1) == 0 ? 1e-6 : odom_reading(1)); // odometry integration pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2)); @@ -219,15 +219,11 @@ int main(int argc, char** argv) laser1Pose.setPose(devicePose); laser1Pose.moveForward(laser_1_pose(0)); myScanner->computeScan(laser1Pose,scan1); - vector<double> scan1d(scan1.begin(), scan1.end()); - Eigen::Map<Eigen::VectorXs> scan1_reading(scan1d.data(), 720); // scan 2 laser2Pose.setPose(devicePose); laser2Pose.moveForward(laser_2_pose(0)); laser2Pose.rt.setEuler( laser2Pose.rt.head()+M_PI, laser2Pose.rt.pitch(), laser2Pose.rt.roll() ); myScanner->computeScan(laser2Pose,scan2); - vector<double> scan2d(scan2.begin(), scan2.end()); - Eigen::Map<Eigen::VectorXs> scan2_reading(scan2d.data(), 720); mean_times(0) += ((double)clock()-t1)/CLOCKS_PER_SEC; @@ -238,8 +234,8 @@ int main(int argc, char** argv) // adding new sensor captures wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading));//, odom_std_factor * Eigen::MatrixXs::Identity(2,2))); // wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3))); - wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1_reading)); - wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2_reading)); + wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1)); + wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2)); // updating problem wolf_manager->update(); mean_times(1) += ((double)clock()-t1)/CLOCKS_PER_SEC; @@ -271,7 +267,7 @@ int main(int argc, char** argv) // draw detected corners std::list<laserscanutils::Corner> corner_list; std::vector<double> corner_vector; - CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1_reading); + 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++ ) { diff --git a/src/examples/test_ceres_laser.cpp b/src/examples/test_ceres_laser.cpp index 49dce88ab0c9c7c3e7598750e004553bc70f41be..424b5c535bd63d648ed6682eaecb34c98bc068d2 100644 --- a/src/examples/test_ceres_laser.cpp +++ b/src/examples/test_ceres_laser.cpp @@ -396,8 +396,6 @@ int main(int argc, char** argv) //compute scan myScan.clear(); myScanner->computeScan(devicePose,myScan); - vector<double> myScanDoubles(myScan.begin(), myScan.end()); - Eigen::Map<Eigen::VectorXs> scan_reading(myScanDoubles.data(), 720); mean_times(0) += ((double)clock()-t1)/CLOCKS_PER_SEC; @@ -408,7 +406,7 @@ int main(int argc, char** argv) time_stamp.setToNow(); wolf_manager->addCapture(new CaptureOdom2D(time_stamp, &odom_sensor, odom_reading, odom_std * Eigen::MatrixXs::Identity(2,2))); //wolf_manager->addCapture(new CaptureGPSFix(time_stamp, &gps_sensor, gps_fix_reading, gps_std * MatrixXs::Identity(3,3))); - wolf_manager->addCapture(new CaptureLaser2D(time_stamp, &laser_sensor, scan_reading)); + wolf_manager->addCapture(new CaptureLaser2D(time_stamp, &laser_sensor, myScan)); // updating problem wolf_manager->update(); @@ -442,7 +440,7 @@ int main(int argc, char** argv) // draw detected corners std::list<laserscanutils::Corner> corner_list; std::vector<double> corner_vector; - CaptureLaser2D last_scan(time_stamp, &laser_sensor, scan_reading); + CaptureLaser2D last_scan(time_stamp, &laser_sensor, myScan); last_scan.extractCorners(corner_list); for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ ) { diff --git a/src/frame_base.cpp b/src/frame_base.cpp index cb75f37b599032344a2da50e87e4af889a435c80..2068eb45f079f296abfda6951367b38358c717fe 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -119,7 +119,7 @@ void FrameBase::setState(const Eigen::VectorXs& _st) (!v_ptr_ ? 0 : v_ptr_->getStateSize()) + (!w_ptr_ ? 0 : w_ptr_->getStateSize())) && "In FrameBase::setState wrong state size"); - assert(!!p_ptr_ && "in FrameBase::setState(), p_ptr_ is nullptr"); + assert(p_ptr_!=nullptr && "in FrameBase::setState(), p_ptr_ is nullptr"); Eigen::Map<Eigen::VectorXs> state_map(p_ptr_->getPtr(), _st.size()); state_map = _st; @@ -132,7 +132,7 @@ void FrameBase::addCapture(CaptureBase* _capt_ptr) void FrameBase::removeCapture(CaptureBaseIter& _capt_iter) { - std::cout << "removing capture " << (*_capt_iter)->nodeId() << " from Frame " << nodeId() << std::endl; + //std::cout << "removing capture " << (*_capt_iter)->nodeId() << " from Frame " << nodeId() << std::endl; removeDownNode(_capt_iter); } diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp index 0e1e0be9d05feb444c37d22ea3c06c451791d601..7529d336da23ab91a25f485373b781c3c0db6ce9 100644 --- a/src/sensor_laser_2D.cpp +++ b/src/sensor_laser_2D.cpp @@ -30,9 +30,9 @@ void SensorLaser2D::setDefaultScanParams() { //TODO: Decide who holds intrinsic parameters, either SensorBase::params_ or scan_params_, but NOTH BOTH!! - scan_params_.angle_min_ = -M_PI/2; - scan_params_.angle_max_ = M_PI/2; - scan_params_.angle_step_ = M_PI/720; + scan_params_.angle_min_ = M_PI/2; + scan_params_.angle_max_ = -M_PI/2; + scan_params_.angle_step_ = -M_PI/720; scan_params_.scan_time_ = 0.01;//not relevant scan_params_.range_min_ = 0.2; scan_params_.range_max_ = 100; diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp index f8f1b62b7410c30eb42938da7d5cc4ff4c8db618..4743190f0578120706d7057d38aa0dac8d29db12 100644 --- a/src/time_stamp.cpp +++ b/src/time_stamp.cpp @@ -14,7 +14,7 @@ TimeStamp::TimeStamp(const WolfScalar _ts) : } TimeStamp::TimeStamp(const unsigned long int _sec, const unsigned long int _nsec) : - time_stamp_((WolfScalar)(_sec + _nsec / 1e9)) + time_stamp_((WolfScalar)_sec + (WolfScalar)_nsec / 1e9) { // } diff --git a/src/wolf_manager.h b/src/wolf_manager.h index a1ec474316014bff7fc0ab9167debd642852fac0..076bbbbcb408f75af306469a018c497c593d65fa 100644 --- a/src/wolf_manager.h +++ b/src/wolf_manager.h @@ -59,7 +59,7 @@ class WolfManager window_size_(_w_size), new_frame_elapsed_time_(_new_frame_elapsed_time) { - createFrame(_init_frame, TimeStamp()); + createFrame(_init_frame, TimeStamp(0)); problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix(); } @@ -95,7 +95,7 @@ class WolfManager void addCapture(CaptureBase* _capture) { new_captures_.push(_capture); - //std::cout << "added new capture: " << _capture->nodeId() << std::endl; + //std::cout << "added new capture: " << _capture->nodeId() << "stamp: " << _capture->getTimeStamp().get() << std::endl; } void update() @@ -110,12 +110,14 @@ class WolfManager if (new_capture->getSensorPtr() == sensor_prior_) { // INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR - //std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl; + //std::cout << "integrating captures " << last_capture_relative_->nodeId() << " " << new_capture->nodeId() << std::endl; last_capture_relative_->integrateCapture((CaptureRelative*)(new_capture)); // NEW KEY FRAME (if enough time from last frame) //std::cout << "new TimeStamp - last Frame TimeStamp = " << new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() << std::endl; - if (new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() > new_frame_elapsed_time_) + if (problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() == 0) + problem_->getTrajectoryPtr()->getFrameListPtr()->back()->setTimeStamp(new_capture->getTimeStamp()); + else if (new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() > new_frame_elapsed_time_) { //std::cout << "store prev frame" << std::endl; auto previous_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back();