diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp index 042c1aa26036e3e5baf95c2081485447b356bb41..68673d2c79f40e69b19352c00788c457873f8e71 100644 --- a/src/capture_laser_2D.cpp +++ b/src/capture_laser_2D.cpp @@ -35,12 +35,15 @@ void CaptureLaser2D::processCapture() std::list<Eigen::Vector4s> corners; //extract corners from range data + //std::cout << "CaptureLaser2D::extractCorners..." << std::endl; extractCorners(corners); //generate a feature for each corner + //std::cout << "CaptureLaser2D::createFeatures..." << std::endl; createFeatures(corners); //Establish constraints for each feature + //std::cout << "CaptureLaser2D::establishConstraints..." << std::endl; establishConstraints(); } @@ -49,281 +52,6 @@ unsigned int CaptureLaser2D::extractCorners(std::list<Eigen::Vector4s> & _corner return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, _corner_list); } -/* -unsigned int CaptureLaser2D::extractCorners_old(std::list<Eigen::Vector4s> & _corner_list) const -{ - //local variables - Eigen::MatrixXs points(3,data_.size()); - double azimuth, cos_theta, theta; - Eigen::Vector3s corner; - Line line; - std::list<Line> line_list; - std::list<Line>::iterator line_it1, line_it2; - std::queue<unsigned int> jumps; - - - //cast to specialized sensor. TODO: Could be done once at the constructor ? - SensorLaser2DPtr laser_ptr = (const SensorLaser2DPtr)sensor_ptr_; - - //init from sensor this->sensor_ptr_ - //double aperture = laser_ptr->getAngleMax() - laser_ptr->getAngleMin(); - double azimuth_step = laser_ptr->getAngleIncrement(); - double range_std_dev = laser_ptr->getRangeStdDev(); - - std::default_random_engine generator(1); - std::uniform_int_distribution<int> rand_window_overlapping(1,segment_window_size); - - //convert range polar data to cartesian points. - for (unsigned int ii = 0; ii<ranges_.size(); ii++) - { - azimuth = laser_ptr->getAngleMax() - azimuth_step * ii; - //std::cout << " azimuth: " << azimuth << std::endl; - points.col(ii) << ranges_(ii)*cos(azimuth), ranges_(ii)*sin(azimuth), 1; //points.row0-> x coordinate, points.row1->y coordinate - - if (ii > 0 && fabs(ranges_(ii)-ranges_(ii-1)) > max_distance) - { - // store jumps - jumps.push(ii); - // consider jumps as a corners -// if (ranges_(ii) < ranges_(ii-1)) -// _corner_list.push_back(Eigen::Vector4s(points(0,ii),points(1,ii),0,0)); // TODO: compute orientation -// else -// _corner_list.push_back(Eigen::Vector4s(points(0,ii-1),points(1,ii-1),0,0));// TODO: compute orientation - } - } - - //find line segments running over the scan - for (unsigned int ii = segment_window_size-1; ii<ranges_.size(); ii=ii+rand_window_overlapping(generator) ) - { - unsigned int i_from = ii - segment_window_size + 1; - - // update the jump to be checked - while (!jumps.empty() && i_from > jumps.front()) - jumps.pop(); - - // check if there is a jump inside the window (not fitting if it is the case) - if (jumps.front() > i_from && jumps.front() <= ii) - continue; - - //Found the best fitting line over points within the window [ii - segment_window_size + 1, ii] - fitLine(i_from, ii, points, line); - - //if error below stdev, add line to result set - if ( line.error < range_std_dev*k_sigmas ) - line_list.push_back(line); - } - - //std::cout << "Lines fitted: " << line_list.size() << std::endl; - - // concatenating and corners only if more than 1 line - if ( line_list.size() > 1 ) - { - // concatenate lines - line_it1 = line_list.begin(); - line_it2 = line_it1; - line_it2 ++; - while ( line_it1 != line_list.end() ) - { - // last of current line and first of next line too far - if (line_it2->first > line_it1->last + max_beam_distance) - { - //std::cout << "lines too far:\nlast of current: " << line_it1->last << " first of next: " << line_it2->first << std::endl; - line_it1 ++; - line_it2 = line_it1; - line_it2 ++; - } - else - { - //compute angle between lines 1 and 2 - cos_theta = (line_it1->vector).dot(line_it2->vector) / ( (line_it1->vector).norm()*(line_it2->vector).norm() ); - theta = acos (cos_theta); - - //TODO: fabs? acos returns [0 PI] - if (theta > M_PI/2) - theta -= M_PI; - // std::cout << std::endl << "cos_theta: " << cos_theta << std::endl << - // "theta: " << theta << std::endl << - // "*index_it1: " << *index_it1 << std::endl << - // "*index_it2: " << *index_it2 << std::endl; - // " (*line_it1).dot(*line_it2): " << (*line_it1).dot(*line_it2) << std::endl << - // " (*line_it1).norm()*(*line_it2).norm(): " << (*line_it1).norm()*(*line_it2).norm() << std::endl; - - - //Check angle threshold and consecutiveness of lines in the scan - if ( fabs(theta) < theta_max_parallel ) - { - Line new_line; - fitLine(line_it1->first, line_it2->last, points, new_line); - if ( new_line.error < range_std_dev*k_sigmas ) - { - *line_it1 = new_line; - line_it2 = line_list.erase(line_it2); - -// std::cout << "lines concatenated" << std::endl; -// std::cout << "line 1: " << std::endl << line_it1->first << std::endl << -// line_it1->last << std::endl << -// line_it1->vector.transpose() << std::endl << -// line_it1->error << std::endl; -// std::cout << "line 2: " << std::endl << line_it2->first << std::endl << -// line_it2->last << std::endl << -// line_it2->vector.transpose() << std::endl << -// line_it2->error << std::endl; -// std::cout << "line resultant: "<< std::endl << new_line.first << std::endl << -// new_line.last << std::endl << -// new_line.vector.transpose() << std::endl << -// new_line.error << std::endl; - } - else - { - //update iterators - line_it1 ++; - line_it2 = line_it1; - line_it2 ++; - } - } - else - { - //update iterators - line_it1 ++; - line_it2 = line_it1; - line_it2 ++; - } - } - } - - //std::cout << "Lines after concatenation: " << line_list.size() << std::endl; - - // find corners over the line list - line_it1 = line_list.begin(); - line_it2 = line_it1; - line_it2 ++; - while ( line_it1 != line_list.end() ) - { - // last of current line and first of next line too far - if (line_it2->first > line_it1->last + max_beam_distance) - { - //std::cout << "lines too far:\nlast of current: " << line_it1->last << " first of next: " << line_it2->first << std::endl; - line_it1 ++; - line_it2 = line_it1; - line_it2 ++; - } - else - { - //compute angle between lines 1 and 2 - cos_theta = (line_it1->vector).dot(line_it2->vector) / ( (line_it1->vector).norm()*(line_it2->vector).norm() ); - theta = acos (cos_theta); - - //TODO: fabs? acos returns [0 PI] - if (theta > M_PI/2) - theta -= M_PI; - -// std::cout << std::endl << "cos_theta: " << cos_theta << std::endl << -// "theta: " << theta << std::endl << -// "line 1: " << line_it1->first << "-" << line_it1->last << std::endl << -// "line 2: " << line_it2->first << "-" << line_it2->last << std::endl << -// " (*line_it1).dot(*line_it2): " << (line_it1->vector).dot(line_it2->vector) << std::endl << -// " (*line_it1).norm()*(*line_it2).norm(): " << (line_it1->vector).norm()*(line_it2->vector).norm() << std::endl; - - - //Check angle threshold and consecutiveness of lines in the scan - if ( fabs(theta) > theta_min ) //TODO: fabs? acos returns [0 PI] - { - corner = (line_it1->vector).cross(line_it2->vector); //cross product between lines is the intersection - corner = corner / corner(2); //normalize point to set last component to 1 - - // Check if the corner is close to both lines ends - if ( (points.col(line_it1->last) - corner).head(2).norm() < max_distance && (points.col(line_it2->first) - corner).head(2).norm() < max_distance) - { - Eigen::Vector3s v1 = (points.col(line_it1->first) - corner); - Eigen::Vector3s v2 = (points.col(line_it2->last) - corner); - v1 /= v1.norm(); - v2 /= v2.norm(); - - //corner orientation - if (v1(0) * v2(1) > v1(1) * v2(0)) - corner(2) = asin (v1(1)); - else - corner(2) = asin (v2(1)); - -// std::cout << "Detected corner!" << std::endl << -// "\tcorner: " << corner.transpose() << std::endl << -// "\tline 1 point: " << points.col(line_it1->first).transpose() << std::endl << -// "\tline 2 point: " << points.col(line_it2->last).transpose() << std::endl << -// "\tv1: " << v1.transpose() << std::endl << -// "\tv2: " << v2.transpose() << std::endl; - - _corner_list.push_back(Eigen::Vector4s(corner(0),corner(1),corner(2),theta)); - } - } - - //update iterators - if (line_it2 != line_list.end() ) - line_it2 ++; - else - { - line_it1 ++; - line_it2 = line_it1; - line_it2 ++; - } - } - } - } - - //std::cout << "Corners extracted: " << _corner_list.size() << std::endl; - - // Erase duplicated corners - if ( _corner_list.size() > 1 ) - { - // concatenate lines - std::list<Eigen::Vector4s>::iterator corner_it1 = _corner_list.begin(); - std::list<Eigen::Vector4s>::iterator corner_it2 = corner_it1; - corner_it2 ++; - while ( corner_it1 != _corner_list.end() ) - { - // Check if two corners are close enough - if ( (*corner_it1 - *corner_it2).head(2).norm() < max_distance ) - { - // TODO: keep the one with larger lines - // compute the mean - *corner_it1 = (*corner_it1 + *corner_it2) / 2; - corner_it2 = _corner_list.erase(corner_it2); - } - else - { - corner_it1 ++; - corner_it2 = corner_it1; - corner_it2 ++; - } - } - } - - //std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl; - - return _corner_list.size(); -} - -void CaptureLaser2D::fitLine(unsigned int _idx_from, unsigned int _idx_to, const Eigen::MatrixXs& _points, Line& line_) const -{ - line_.first = _idx_from; - line_.last = _idx_to; - - //Found the best fitting line over points within the window. Build the system: A*line=[0 0 1]'. Matrix A = a_ij - Eigen::Matrix3s AA = _points.block(0, _idx_from, 3, _idx_to-_idx_from+1) * _points.block(0, _idx_from, 3, _idx_to-_idx_from+1).transpose(); - AA.row(2) << 0,0,1; - - //solve for line - line_.vector = AA.inverse().col(2); - - // compute fitting error -// line_.error = 0; -// for(unsigned int jj = _idx_from; jj<=_idx_to; jj++) -// line_.error += fabs( line_.vector(0)*_points(0,jj) + line_.vector(1)*_points(1,jj) + line_.vector(2)) / sqrt( line_.vector(0)*line_.vector(0) + line_.vector(1)*line_.vector(1) ); -// line_.error /= (_idx_to-_idx_from+1); - - line_.error = (_points.block(0, _idx_from, 3, _idx_to-_idx_from+1).transpose() * line_.vector).array().abs().sum()/(line_.vector.head(2).norm()*(_idx_to-_idx_from+1)); -} -*/ - void CaptureLaser2D::createFeatures(std::list<Eigen::Vector4s> & _corner_list) { Eigen::Matrix4s cov_mat; @@ -336,23 +64,30 @@ void CaptureLaser2D::createFeatures(std::list<Eigen::Vector4s> & _corner_list) //for each corner in the list create a feature for (auto corner_it = _corner_list.begin(); corner_it != _corner_list.end(); corner_it ++) + { + (*corner_it)(2) = 0; this->addFeature( (FeatureBase*)(new FeatureCorner2D( (*corner_it), cov_mat ) ) ); + } } void CaptureLaser2D::establishConstraints() { // Global transformation TODO: Change by a function - Eigen::Vector2s t(*getFramePtr()->getPPtr()->getPtr(),*(getFramePtr()->getPPtr()->getPtr()+1)); + Eigen::Vector2s t_robot(*getFramePtr()->getPPtr()->getPtr(),*(getFramePtr()->getPPtr()->getPtr()+1)); WolfScalar o = *(getFramePtr()->getOPtr()->getPtr()); - Eigen::Matrix2s R; + Eigen::Matrix2s R_robot; if (getFramePtr()->getOPtr()->getStateType() == ST_THETA) - R << cos(o),-sin(o), + R_robot << cos(o),-sin(o), sin(o), cos(o); else { //TODO } + // Sensor transformation + Eigen::Vector2s t_sensor = getSensorPtr()->getSensorPosition()->head(2); + Eigen::Matrix2s R_sensor = getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>().transpose(); + //Brute force closest (xy and theta) landmark search // std::cout << "Brute force closest (xy and theta) landmark search: N features:" << getFeatureListPtr()->size() << std::endl; // std::cout << "N landmark:" << getTop()->getMapPtr()->getLandmarkListPtr()->size() << std::endl; @@ -361,17 +96,18 @@ void CaptureLaser2D::establishConstraints() // std::cout << "rot:" << R << std::endl; for (auto feature_it = getFeatureListPtr()->begin(); feature_it != getFeatureListPtr()->end(); feature_it++ ) { - double max_distance_matching2 = 0.1; //TODO: max_distance_matching depending on localization and landmarks uncertainty - double max_theta_matching = 0.02; //TODO: max_theta_matching depending on localization and landmarks uncertainty + double max_distance_matching2 = 0.5; //TODO: max_distance_matching depending on localization and landmarks uncertainty + double max_theta_matching = 0.2; //TODO: max_theta_matching depending on localization and landmarks uncertainty //Find the closest landmark to the feature LandmarkCorner2D* correspondent_landmark = nullptr; Eigen::Map<Eigen::Vector2s> feature_position((*feature_it)->getMeasurementPtr()->data()); Eigen::Map<Eigen::Vector1s> feature_orientation = ((*feature_it)->getMeasurementPtr()->data()+2); - Eigen::Vector2s feature_global_position = R*feature_position + t; + Eigen::Vector2s feature_global_position = R_robot * (R_sensor * feature_position + t_sensor) + t_robot; Eigen::Vector1s feature_global_orientation; - feature_global_orientation(0) = feature_orientation(0) + o; + feature_global_orientation(0) = feature_orientation(0) + o + atan2(R_sensor(1,0),R_sensor(0,0)); + //feature_global_orientation(0) = 0; double min_distance2 = max_distance_matching2; // std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl; @@ -383,13 +119,28 @@ void CaptureLaser2D::establishConstraints() WolfScalar landmark_orientation = *((*landmark_it)->getOPtr()->getPtr()); WolfScalar distance2 = (landmark_position-feature_global_position).transpose() * (landmark_position-feature_global_position); - if (distance2 < min_distance2 && fabs(landmark_orientation-feature_global_orientation(0) < max_theta_matching)) + WolfScalar theta_distance = fabs(landmark_orientation-feature_global_orientation(0)); + + if (theta_distance > M_PI) + theta_distance -= 2 * M_PI; + if (distance2 < min_distance2) { -// std::cout << "Landmark found: " << (*landmark_it)->nodeId() << std::endl; -// std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl; - correspondent_landmark = (LandmarkCorner2D*)(*landmark_it); - min_distance2 = distance2; + if (theta_distance < max_theta_matching) + { +// std::cout << "Position & orientation near landmark found: " << (*landmark_it)->nodeId() << std::endl; +// std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl; + + correspondent_landmark = (LandmarkCorner2D*)(*landmark_it); + min_distance2 = distance2; + } + else + { + std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl; + std::cout << "global position:" << feature_global_position.transpose() << " orientation:" << feature_global_orientation << std::endl; + std::cout << "Landmark with near position but wrong orientation: " << (*landmark_it)->nodeId() << std::endl; + std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl; + } } } if (correspondent_landmark == nullptr) @@ -404,8 +155,8 @@ void CaptureLaser2D::establishConstraints() LandmarkBase* corr_landmark(correspondent_landmark); getTop()->getMapPtr()->addLandmark(corr_landmark); -// std::cout << "Landmark created: " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << std::endl; -// std::cout << "global position: " << *new_landmark->getPPtr()->getPtr() << " " << *(new_landmark->getPPtr()->getPtr()+1) << " orientation:" << *new_landmark->getOPtr()->getPtr() << std::endl; + std::cout << "Landmark created: " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << std::endl; + std::cout << "global position: " << *corr_landmark->getPPtr()->getPtr() << " " << *(corr_landmark->getPPtr()->getPtr()+1) << " orientation:" << *corr_landmark->getOPtr()->getPtr() << std::endl; } else correspondent_landmark->hit(); @@ -413,12 +164,6 @@ void CaptureLaser2D::establishConstraints() //std::cout << "Creating new constraint: Landmark " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << " & feature " << (*feature_it)->nodeId() << std::endl; // Add constraint to the correspondent landmark -// ConstraintBase* landmark_constraint(new ConstraintCorner2DTheta(&(*feature_it), -// correspondent_landmark, -// getFramePtr()->getPPtr(),//_robotPPtr, -// getFramePtr()->getOPtr(),//_robotOPtr, -// correspondent_landmark->getPPtr(), //_landmarkPPtr, -// correspondent_landmark->getOPtr())); //_landmarkOPtr, (*feature_it)->addConstraint(new ConstraintCorner2DTheta(*feature_it, correspondent_landmark, getFramePtr()->getPPtr(),//_robotPPtr, diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp index 0626cd2a5a4433f736ee85ff395686bdebaddb1b..b4736054c366c367a1720e663b3273f53c2b8b8f 100644 --- a/src/capture_odom_2D.cpp +++ b/src/capture_odom_2D.cpp @@ -25,9 +25,11 @@ CaptureOdom2D::~CaptureOdom2D() inline void CaptureOdom2D::processCapture() { + //std::cout << "CaptureOdom2D::addFeature..." << std::endl; // ADD FEATURE addFeature(new FeatureOdom2D(data_,data_covariance_)); + //std::cout << "CaptureOdom2D::addConstraints..." << std::endl; // ADD CONSTRAINT addConstraints(); } diff --git a/src/constraint_corner_2D_theta.h b/src/constraint_corner_2D_theta.h index 1ab409b7689ff3dc462b2d44ff37f099e4aa0ca5..60d4a5fbe9f3fb0a16f174884e126464a135628c 100644 --- a/src/constraint_corner_2D_theta.h +++ b/src/constraint_corner_2D_theta.h @@ -1,4 +1,3 @@ - #ifndef CONSTRAINT_CORNER_2D_THETA_H_ #define CONSTRAINT_CORNER_2D_THETA_H_ @@ -46,23 +45,39 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1> template <typename T> bool operator()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP, const T* const _landmarkO, T* _residuals) const { - //TODO: Not compute every step InvRot + //TODO: Not computing the transformations each iteration // Mapping Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position(_landmarkP); Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position(_robotP); - Eigen::Matrix<T,2,2> InvRot; - InvRot << cos(*_robotO), sin(*_robotO), - -sin(*_robotO), cos(*_robotO); +// std::cout << "getSensorPosition: " << std::endl; +// std::cout << getCapturePtr()->getSensorPtr()->getSensorPosition()->head(2).transpose() << std::endl; +// std::cout << "getSensorRotation: " << std::endl; +// std::cout << getCapturePtr()->getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>() << std::endl; +// std::cout << "atan2: " << atan2(getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,1),getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,0)) << std::endl; + + + // sensor transformation + Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getSensorPosition()->head(2).cast<T>(); + Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>().transpose()).cast<T>(); + + Eigen::Matrix<T,2,2> inverse_R_robot; + inverse_R_robot << cos(*_robotO), sin(*_robotO), + -sin(*_robotO), cos(*_robotO); // Expected measurement - Eigen::Matrix<T,2,1> expected_landmark_relative_position = InvRot * (landmark_position - robot_position); - T expected_landmark_relative_orientation = (*_landmarkO) - (*_robotO); + Eigen::Matrix<T,2,1> expected_landmark_relative_position = inverse_R_sensor * (inverse_R_robot * (landmark_position - robot_position) - sensor_position); + T expected_landmark_relative_orientation = (*_landmarkO) - (*_robotO) - atan2(inverse_R_sensor(0,1),inverse_R_sensor(0,0)); + + while (expected_landmark_relative_orientation > T(M_PI)) + expected_landmark_relative_orientation = expected_landmark_relative_orientation - T(2*M_PI); + while (expected_landmark_relative_orientation <= T(-M_PI)) + expected_landmark_relative_orientation = expected_landmark_relative_orientation + T(2*M_PI); // Residuals _residuals[0] = (expected_landmark_relative_position(0) - T((*measurement_ptr_)(0))) / T((*measurement_covariance_ptr_)(0,0)); _residuals[1] = (expected_landmark_relative_position(1) - T((*measurement_ptr_)(1))) / T((*measurement_covariance_ptr_)(1,1)); - _residuals[2] = (expected_landmark_relative_orientation - T((*measurement_ptr_)(2))) / T((*measurement_covariance_ptr_)(2,2)); + _residuals[2] = (expected_landmark_relative_orientation - T((*measurement_ptr_)(2))) / T(100*(*measurement_covariance_ptr_)(2,2)); // std::cout << "\nCONSTRAINT: " << nodeId() << std::endl; // std::cout << "Feature: " << getFeaturePtr()->nodeId() << std::endl; diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 3032761c4f03d3683490047b446a0ea7cecd5051..1d9713765430ad1a450822f4d8ae13f272b46e32 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -51,7 +51,12 @@ IF(faramotics_FOUND) IF (laser_scan_utils_FOUND) ADD_EXECUTABLE(test_ceres_laser test_ceres_laser.cpp) TARGET_LINK_LIBRARIES(test_ceres_laser -# ${GLUT_glut_LIBRARY} + ${pose_state_time_LIBRARIES} + ${faramotics_LIBRARIES} + ${PROJECT_NAME}) + + ADD_EXECUTABLE(test_ceres_2_lasers test_ceres_2_lasers.cpp) + TARGET_LINK_LIBRARIES(test_ceres_2_lasers ${pose_state_time_LIBRARIES} ${faramotics_LIBRARIES} ${PROJECT_NAME}) diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp new file mode 100644 index 0000000000000000000000000000000000000000..165566c201af24871dbcbb4d80ea81ace5c3b4c8 --- /dev/null +++ b/src/examples/test_ceres_2_lasers.cpp @@ -0,0 +1,622 @@ +//std includes +#include <cstdlib> +#include <iostream> +#include <fstream> +#include <memory> +#include <random> +#include <typeinfo> +#include <ctime> +#include <queue> + +// Eigen includes +#include <eigen3/Eigen/Dense> +#include <eigen3/Eigen/Geometry> + +//Ceres includes +#include "ceres/ceres.h" +#include "glog/logging.h" + +//Wolf includes +#include "wolf.h" +#include "sensor_base.h" +#include "sensor_odom_2D.h" +#include "sensor_gps_fix.h" +#include "feature_base.h" +#include "frame_base.h" +#include "state_point.h" +#include "state_complex_angle.h" +#include "capture_base.h" +#include "capture_relative.h" +#include "capture_odom_2D.h" +#include "capture_gps_fix.h" +#include "capture_laser_2D.h" +#include "state_base.h" +#include "constraint_sparse.h" +#include "constraint_gps_2D.h" +#include "constraint_odom_2D_theta.h" +#include "constraint_odom_2D_complex_angle.h" +#include "trajectory_base.h" +#include "map_base.h" +#include "wolf_problem.h" + +// ceres wrapper include +#include "ceres_wrapper/ceres_manager.h" + +//C includes for sleep, time and main args +#include "unistd.h" + +//faramotics includes +#include "faramotics/dynamicSceneRender.h" +#include "faramotics/rangeScan2D.h" +#include "btr-headers/pose3d.h" + +//function travel around +void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_) +{ + if (ii<=120) + { + displacement_ = 0.1; + rotation_ = 0; + } + else if ( (ii>120) && (ii<=170) ) + { + displacement_ = 0.2; + rotation_ = 1.8*M_PI/180; + } + else if ( (ii>170) && (ii<=220) ) + { + displacement_ = 0; + rotation_ = -1.8*M_PI/180; + } + else if ( (ii>220) && (ii<=310) ) + { + displacement_ = 0.1; + rotation_ = 0; + } + else if ( (ii>310) && (ii<=487) ) + { + displacement_ = 0.1; + rotation_ = -1.*M_PI/180; + } + else if ( (ii>487) && (ii<=600) ) + { + displacement_ = 0.2; + rotation_ = 0; + } + else if ( (ii>600) && (ii<=700) ) + { + displacement_ = 0.1; + rotation_ = -1.*M_PI/180; + } + else if ( (ii>700) && (ii<=780) ) + { + displacement_ = 0; + rotation_ = -1.*M_PI/180; + } + else + { + displacement_ = 0.3; + rotation_ = 0.0*M_PI/180; + } + + pose.moveForward(displacement_); + pose.rt.setEuler( pose.rt.head()+rotation_, pose.rt.pitch(), pose.rt.roll() ); +} + +class WolfManager +{ + protected: + bool use_complex_angles_; + WolfProblem* problem_; + std::queue<CaptureBase*> new_captures_; + SensorBase* sensor_prior_; + unsigned int window_size_; + FrameBaseIter first_window_frame_; + + public: + WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length, const unsigned int& _w_size=10) : + use_complex_angles_(_complex_angle), + problem_(new WolfProblem(_state_length)), + sensor_prior_(_sensor_prior), + window_size_(_w_size) + { + Eigen::VectorXs init_frame(use_complex_angles_ ? 4 : 3); + if (use_complex_angles_) + init_frame << 2, 8, 1, 0; + else + init_frame << 2, 8, 0; + createFrame(init_frame, 0); + } + + virtual ~WolfManager() + { + delete problem_; + } + + void createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp) + { + // Create frame and add it to the trajectory + StateBase* new_position = new StatePoint2D(problem_->getNewStatePtr()); + problem_->addState(new_position, _frame_state.head(2)); + + StateBase* new_orientation; + if (use_complex_angles_) + new_orientation = new StateComplexAngle(problem_->getNewStatePtr()); + else + new_orientation = new StateTheta(problem_->getNewStatePtr()); + + problem_->addState(new_orientation, _frame_state.tail(new_orientation->getStateSize())); + + problem_->getTrajectoryPtr()->addFrame(new FrameBase(_time_stamp, new_position, new_orientation)); + + // add a zero odometry capture (in order to integrate) + problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new CaptureOdom2D(_time_stamp, + sensor_prior_, + Eigen::Vector2s::Zero(), + Eigen::Matrix2s::Zero())); + } + + void addCapture(CaptureBase* _capture) + { + new_captures_.push(_capture); + //std::cout << "added new capture: " << _capture->nodeId() << std::endl; + } + + void update() + { + while (!new_captures_.empty()) + { + // EXTRACT NEW CAPTURE + CaptureBase* new_capture = new_captures_.front(); + new_captures_.pop(); + + // ODOMETRY SENSOR + if (new_capture->getSensorPtr() == sensor_prior_) + { + // FIND PREVIOUS RELATIVE CAPTURE + CaptureRelative* previous_relative_capture = nullptr; + for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++) + { + //std::cout << "capture: " << (*capture_it)->nodeId() << std::endl; + if ((*capture_it)->getSensorPtr() == sensor_prior_) + { + previous_relative_capture = (CaptureRelative*)(*capture_it); + break; + } + } + + // INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR + //std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl; + previous_relative_capture->integrateCapture((CaptureRelative*)(new_capture)); + Eigen::VectorXs prior = previous_relative_capture->computePrior(); + + // 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() > 0.1) + { + //std::cout << "store prev frame" << std::endl; + auto previous_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back(); + + // NEW FRAME + //std::cout << "new frame" << std::endl; + createFrame(prior, new_capture->getTimeStamp()); + + // COMPUTE PREVIOUS FRAME CAPTURES + //std::cout << "compute prev frame" << std::endl; + for (auto capture_it = previous_frame_ptr->getCaptureListPtr()->begin(); capture_it != previous_frame_ptr->getCaptureListPtr()->end(); capture_it++) + (*capture_it)->processCapture(); + + // WINDOW of FRAMES (remove or fix old frames) + //std::cout << "frame window" << std::endl; + if (problem_->getTrajectoryPtr()->getFrameListPtr()->size() > window_size_) + { + //std::cout << "frame fixing" << std::endl; + //problem_->getTrajectoryPtr()->removeFrame(problem_->getTrajectoryPtr()->getFrameListPtr()->begin()); + (*first_window_frame_)->fix(); + first_window_frame_++; + } + else + first_window_frame_ = problem_->getTrajectoryPtr()->getFrameListPtr()->begin(); + } + } + else + { + // ADD CAPTURE TO THE LAST FRAME (or substitute the same sensor previous capture) + //std::cout << "adding not odometry capture..." << std::endl; + for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++) + { + if ((*capture_it)->getSensorPtr() == new_capture->getSensorPtr()) + { + //std::cout << "removing previous capture" << std::endl; + problem_->getTrajectoryPtr()->getFrameListPtr()->back()->removeCapture(capture_it); + //std::cout << "removed!" << std::endl; + break; + } + } + problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new_capture); + } + } + } + + const Eigen::VectorXs getState() const + { + return problem_->getState(); + } + + StateBaseList* getStateListPtr() + { + return problem_->getStateListPtr(); + } + + Eigen::VectorXs getVehiclePose() + { + return Eigen::Map<Eigen::VectorXs>(problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr()->getPtr(), use_complex_angles_ ? 4 : 3); + } + + void getConstraintsList(ConstraintBaseList& corr_list) + { + problem_->getTrajectoryPtr()->getConstraintList(corr_list); + } + + WolfProblem* getProblemPtr() + { + return problem_; + } + + void printTree() + { + problem_->print(); + } +}; + +int main(int argc, char** argv) +{ + std::cout << "\n ========= 2D Robot with odometry and GPS ===========\n"; + + // USER INPUT ============================================================================================ + if (argc!=4 || atoi(argv[1])<1 || atoi(argv[1])>1100 || atoi(argv[2]) < 0 || atoi(argv[3]) < 0 || atoi(argv[3]) > 1) + { + std::cout << "Please call me with: [./test_ceres_manager NI PRINT ORIENTATION_MODE], where:" << std::endl; + std::cout << " - NI is the number of iterations (0 < NI < 1100)" << std::endl; + std::cout << " - WS is the window size (0 < WS)" << std::endl; + std::cout << " - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl; + std::cout << "EXIT due to bad user input" << std::endl << std::endl; + return -1; + } + + clock_t t1, t2; + t1=clock(); + + unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution + unsigned int window_size = (unsigned int) atoi(argv[2]); + bool complex_angle = (bool) atoi(argv[3]); + + // INITIALIZATION ============================================================================================ + //init random generators + WolfScalar odom_std= 0.01; + WolfScalar gps_std= 1; + std::default_random_engine generator(1); + std::normal_distribution<WolfScalar> distribution_odom(0.0, odom_std); //odometry noise + std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise + + //init google log + google::InitGoogleLogging(argv[0]); + + // Ceres initialization + ceres::Solver::Options ceres_options; + ceres_options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION; + ceres_options.max_line_search_step_contraction = 1e-3; + // ceres_options.minimizer_progress_to_stdout = false; + // ceres_options.line_search_direction_type = ceres::LBFGS; + // ceres_options.max_num_iterations = 100; + ceres::Problem::Options problem_options; + problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; + CeresManager* ceres_manager = new CeresManager(problem_options); + std::ofstream log_file, landmark_file; //output file + + // Faramotics stuff + Cpose3d viewPoint; + Cpose3d devicePose, laser1Pose, laser2Pose; + vector<Cpose3d> devicePoses; + vector<float> scan1, scan2; + string modelFileName; + + //model and initial view point + modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"; + //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj"; + //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj"; + devicePose.setPose(2,8,0.2,0,0,0); + viewPoint.setPose(devicePose); + viewPoint.moveForward(10); + viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/2, viewPoint.rt.pitch()+30.*M_PI/180., viewPoint.rt.roll() ); + viewPoint.moveForward(-15); + //glut initialization + //glutInit(&argc, argv); + faramotics::initGLUT(argc, argv); + + //create a viewer for the 3D model and scan points + CdynamicSceneRender* myRender = new CdynamicSceneRender(1200,700,90*M_PI/180,90*700.0*M_PI/(1200.0*180.0),0.2,100); + myRender->loadAssimpModel(modelFileName,true); //with wireframe + //create scanner and load 3D model + CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG);//HOKUYO_UTM30LX_180DEG or LEUZE_RS4 + myScanner->loadAssimpModel(modelFileName); + + //variables + Eigen::Vector2s odom_reading, gps_fix_reading; + Eigen::VectorXs odom_inc_true(n_execution*2);//invented motion + Eigen::VectorXs pose_odom(3); //current odometry integred pose + Eigen::VectorXs ground_truth(n_execution*3); //all true poses + Eigen::VectorXs odom_trajectory(n_execution*3); //open loop trajectory + Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings + Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings + TimeStamp time_stamp; + Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7); + + // Wolf manager initialization + SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(6,1), odom_std, odom_std); + //SensorGPSFix gps_sensor(Eigen::MatrixXs::Zero(6,1), gps_std); + Eigen::Vector6s laser_1_pose, laser_2_pose; + laser_1_pose << 1.2,0,0,0,0,0; //laser 1 + laser_2_pose << -1.2,0,0,0,0,M_PI; //laser 2 + SensorLaser2D laser_1_sensor(laser_1_pose); + SensorLaser2D laser_2_sensor(laser_2_pose); + WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e9, window_size); + + // Initial pose + pose_odom << 2,8,0; + ground_truth.head(3) = pose_odom; + odom_trajectory.head(3) = pose_odom; + + std::cout << "START TRAJECTORY..." << std::endl; + // START TRAJECTORY ============================================================================================ + for (uint step=1; step < n_execution; step++) + { + //get init time + t2=clock(); + + // ROBOT MOVEMENT --------------------------- + //std::cout << "ROBOT MOVEMENT..." << std::endl; + // moves the device position + t1=clock(); + motionCampus(step, devicePose, odom_reading(0), odom_reading(1)); + devicePoses.push_back(devicePose); + + + // SENSOR DATA --------------------------- + //std::cout << "SENSOR DATA..." << std::endl; + // store groundtruth + 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(1) += distribution_odom(generator); + + // odometry integration + pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2)); + pose_odom(1) = pose_odom(1) + odom_reading(0) * sin(pose_odom(2)); + pose_odom(2) = pose_odom(2) + odom_reading(1); + odom_trajectory.segment(step*3,3) = pose_odom; + + // compute GPS + //gps_fix_reading << devicePose.pt(0), devicePose.pt(1), 0; + //gps_fix_reading(0) += distribution_gps(generator); + //gps_fix_reading(1) += distribution_gps(generator); + + //compute scans + scan1.clear(); + scan2.clear(); + // scan 1 + 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; + + + // ADD CAPTURES --------------------------- + //std::cout << "ADD CAPTURES..." << std::endl; + t1=clock(); + // adding new sensor captures + 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_1_sensor, scan1_reading)); + wolf_manager->addCapture(new CaptureLaser2D(time_stamp, &laser_2_sensor, scan2_reading)); + + // updating problem + wolf_manager->update(); + mean_times(1) += ((double)clock()-t1)/CLOCKS_PER_SEC; + + + // UPDATING CERES --------------------------- + //std::cout << "UPDATING CERES..." << std::endl; + t1=clock(); + // update state units and constraints in ceres + ceres_manager->update(wolf_manager->getProblemPtr()); + mean_times(2) += ((double)clock()-t1)/CLOCKS_PER_SEC; + + + // SOLVE OPTIMIZATION --------------------------- + std::cout << "SOLVING..." << std::endl; + t1=clock(); + ceres::Solver::Summary summary = ceres_manager->solve(ceres_options); + //std::cout << summary.FullReport() << std::endl; + mean_times(3) += ((double)clock()-t1)/CLOCKS_PER_SEC; + + // COMPUTE COVARIANCES --------------------------- + //std::cout << "COMPUTING COVARIANCES..." << std::endl; + t1=clock(); +// if (step > 2) +// ceres_manager->computeCovariances(wolf_manager->getProblemPtr()->getStateListPtr(), wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr()); + mean_times(4) += ((double)clock()-t1)/CLOCKS_PER_SEC; + + // DRAWING STUFF --------------------------- + t1=clock(); + // draw detected corners + std::list<Eigen::Vector4s> corner_list; + std::vector<double> corner_vector; + CaptureLaser2D last_scan(time_stamp, &laser_1_sensor, scan1_reading); + last_scan.extractCorners(corner_list); + for (std::list<Eigen::Vector4s>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ ) + { + corner_vector.push_back(corner_it->x()); + corner_vector.push_back(corner_it->y()); + } + myRender->drawCorners(laser1Pose,corner_vector); + + // draw landmarks + 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(); + landmark_vector.push_back(*position_ptr); //x + landmark_vector.push_back(*(position_ptr+1)); //y + landmark_vector.push_back(0.2); //z + } + myRender->drawLandmarks(landmark_vector); + + // draw localization and sensors + Cpose3d estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose; + estimated_vehicle_pose.setPose(wolf_manager->getVehiclePose()(0),wolf_manager->getVehiclePose()(1),0.2,wolf_manager->getVehiclePose()(2),0,0); + estimated_laser_1_pose.setPose(estimated_vehicle_pose); + estimated_laser_1_pose.moveForward(laser_1_pose(0)); + estimated_laser_2_pose.setPose(estimated_vehicle_pose); + estimated_laser_2_pose.moveForward(laser_2_pose(0)); + estimated_laser_2_pose.rt.setEuler( estimated_laser_2_pose.rt.head()+M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll() ); + std::vector<Cpose3d> estimated_poses_vector{estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose}; + myRender->drawPoseAxisVector(estimated_poses_vector); + + //Set view point and render the scene + //locate visualization view point, somewhere behind the device +// viewPoint.setPose(devicePose); +// viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() ); +// viewPoint.moveForward(-5); + myRender->setViewPoint(viewPoint); + myRender->drawPoseAxis(devicePose); + myRender->drawScan(laser1Pose,scan1,180.*M_PI/180.,90.*M_PI/180.); //draw scan + myRender->render(); + mean_times(5) += ((double)clock()-t1)/CLOCKS_PER_SEC; + + // TIME MANAGEMENT --------------------------- + double dt = ((double)clock()-t2)/CLOCKS_PER_SEC; + mean_times(6) += dt; + + if (dt < 0.1) + usleep(100000-1e6*dt); + +// std::cout << "\nTree after step..." << std::endl; +// wolf_manager->printTree(); + } + + // DISPLAY RESULTS ============================================================================================ + mean_times /= n_execution; + std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl; + std::cout << " data generation: " << mean_times(0) << std::endl; + std::cout << " wolf managing: " << mean_times(1) << std::endl; + std::cout << " ceres managing: " << mean_times(2) << std::endl; + std::cout << " ceres optimization: " << mean_times(3) << std::endl; + std::cout << " ceres covariance: " << mean_times(4) << std::endl; + std::cout << " results drawing: " << mean_times(5) << std::endl; + std::cout << " loop time: " << mean_times(6) << std::endl; + +// std::cout << "\nTree before deleting..." << std::endl; +// wolf_manager->printTree(); + + // Draw Final result ------------------------- + 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(); + landmark_vector.push_back(*position_ptr); //x + landmark_vector.push_back(*(position_ptr+1)); //y + landmark_vector.push_back(0.2); //z + } + myRender->drawLandmarks(landmark_vector); +// viewPoint.setPose(devicePoses.front()); +// viewPoint.moveForward(10); +// viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() ); +// viewPoint.moveForward(-10); + myRender->setViewPoint(viewPoint); + myRender->render(); + + // Print Final result in a file ------------------------- + // Vehicle poses + int i = 0; + Eigen::VectorXs state_poses(n_execution * 3); + for (auto frame_it = wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->begin(); frame_it != wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->end(); frame_it++ ) + { + if (complex_angle) + state_poses.segment(i,3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr()+1), atan2(*(*frame_it)->getOPtr()->getPtr(), *((*frame_it)->getOPtr()->getPtr()+1)); + else + state_poses.segment(i,3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr()+1), *(*frame_it)->getOPtr()->getPtr(); + i+=3; + } + + // Landmarks + i = 0; + Eigen::VectorXs landmarks(wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->size()*2); + for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ ) + { + Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getPPtr()->getPtr()); + landmarks.segment(i,2) = landmark; + i+=2; + } + + // Print log files + std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt")); + log_file.open(filepath, std::ofstream::out); //open log file + + if (log_file.is_open()) + { + log_file << 0 << std::endl; + for (unsigned int ii = 0; ii<n_execution; ii++) + log_file << state_poses.segment(ii*3,3).transpose() + << "\t" << ground_truth.segment(ii*3,3).transpose() + << "\t" << (state_poses.segment(ii*3,3)-ground_truth.segment(ii*3,3)).transpose() + << "\t" << odom_trajectory.segment(ii*3,3).transpose() + << "\t" << gps_fix_readings.segment(ii*3,3).transpose() << std::endl; + log_file.close(); //close log file + std::cout << std::endl << "Result file " << filepath << std::endl; + } + else + std::cout << std::endl << "Failed to write the log file " << filepath << std::endl; + + std::string filepath2 = getenv("HOME") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt") : std::string("/Desktop/landmarks_file_2.txt")); + landmark_file.open(filepath2, std::ofstream::out); //open log file + + if (landmark_file.is_open()) + { + for (unsigned int ii = 0; ii<landmarks.size(); ii+=2) + landmark_file << landmarks.segment(ii,2).transpose() << std::endl; + landmark_file.close(); //close log file + std::cout << std::endl << "Landmark file " << filepath << std::endl; + } + else + std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl; + + std::cout << "Press any key for ending... " << std::endl << std::endl; + std::getchar(); + + delete myRender; + delete myScanner; + delete wolf_manager; + std::cout << "wolf deleted" << std::endl; + delete ceres_manager; + std::cout << "ceres_manager deleted" << std::endl; + + std::cout << " ========= END ===========" << std::endl << std::endl; + + //exit + return 0; +} diff --git a/src/examples/test_ceres_manager.cpp b/src/examples/test_ceres_manager.cpp index 7ed716701b3efef822494be5d9940c21b5971588..64e1a2f5f96dfdafbd16e8da825d9a8e6775fb72 100644 --- a/src/examples/test_ceres_manager.cpp +++ b/src/examples/test_ceres_manager.cpp @@ -709,8 +709,8 @@ int main(int argc, char** argv) Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings std::queue<StateBase*> new_state_units; // new state units in wolf that must be added to ceres std::queue<ConstraintXBase*> new_constraints; // new constraints in wolf that must be added to ceres - SensorBase* odom_sensor = new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(3,1),0); - SensorBase* gps_sensor = new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(3,1),0); + SensorBase* odom_sensor = new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(6,1),0); + SensorBase* gps_sensor = new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(6,1),0); // Initial pose pose_true << 0,0,0; diff --git a/src/examples/test_ceres_manager_tree.cpp b/src/examples/test_ceres_manager_tree.cpp index ebaba3b404d1452ae5ba2a929f7bc77b756c52d8..c475bc9dd348216332d651c0b55670da8ecd4d1e 100644 --- a/src/examples/test_ceres_manager_tree.cpp +++ b/src/examples/test_ceres_manager_tree.cpp @@ -120,22 +120,23 @@ class WolfManager // TODO: accumulate odometries if (new_capture->getSensorPtr() == sensor_prior_) { - createFrame(VectorXs::Zero(use_complex_angles_ ? 4 : 3), new_capture->getTimeStamp()); - - // ADD CAPTURE TO THE NEW FRAME + // ADD CAPTURE TO THE PREVIOUS FRAME trajectory_->getFrameListPtr()->back()->addCapture(new_capture); // COMPUTE PRIOR - trajectory_->getFrameListPtr()->back()->setState(new_capture->computePrior()); + trajectory_->getFrameListPtr()->back()->setState(new_capture->computePrior()); + + // CREATE NEW FRAME + createFrame(VectorXs::Zero(use_complex_angles_ ? 4 : 3), new_capture->getTimeStamp()); - // TODO: Change by something like... + // NEW STATE UNITS TO BE ADDED BY CERES //new_state_units.insert(new_state_units.end(), trajectory_.getFrameList.back()->getStateList().begin(), trajectory_.getFrameList.back()->getStateList().end()); new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getPPtr()); new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getOPtr()); } else { - // ADD CAPTURE TO THE NEW FRAME + // ADD CAPTURE TO THE LAST FRAME trajectory_->getFrameListPtr()->back()->addCapture(new_capture); } @@ -253,8 +254,8 @@ int main(int argc, char** argv) std::list<ConstraintBase*> new_constraints; // new constraints in wolf that must be added to ceres // Wolf manager initialization - SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(3,1), odom_std, odom_std); - SensorGPSFix gps_sensor(Eigen::MatrixXs::Zero(3,1), gps_std); + SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(6,1), odom_std, odom_std); + SensorGPSFix gps_sensor(Eigen::MatrixXs::Zero(6,1), gps_std); WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, n_execution * (complex_angle ? 4 : 3)); // Initial pose diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp index cf687801feed8411010134cf19153836e3f8e81d..7c2bafe8e2bcfc494bf65706caf543723c29d30f 100644 --- a/src/sensor_base.cpp +++ b/src/sensor_base.cpp @@ -1,21 +1,26 @@ #include "sensor_base.h" -SensorBase::SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, const Eigen::VectorXs & _params) : +SensorBase::SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, const Eigen::VectorXs & _params) : NodeBase("SENSOR"), type_(_tp), - sensor_pose_vehicle_(_pose), + sensor_position_vehicle_(_pose.head(3)), params_(_params.size()) { params_ = _params; + sensor_rotation_vehicle_ = Eigen::AngleAxisd(_pose(3), Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(_pose(4), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(_pose(5), Eigen::Vector3d::UnitZ()); } -SensorBase::SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, unsigned int _params_size) : +SensorBase::SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, unsigned int _params_size) : NodeBase("SENSOR"), type_(_tp), - sensor_pose_vehicle_(_pose), + sensor_position_vehicle_(_pose.head(3)), params_(_params_size) { - // + sensor_rotation_vehicle_ = Eigen::AngleAxisd(_pose(3), Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(_pose(4), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(_pose(5), Eigen::Vector3d::UnitZ()); } SensorBase::~SensorBase() @@ -28,8 +33,16 @@ const SensorType SensorBase::getSensorType() const return type_; } -const Eigen::VectorXs * SensorBase::getSensorPose() const +const Eigen::Vector3s * SensorBase::getSensorPosition() const +{ + //std::cout << "getSensorPosition: " << sensor_position_vehicle_.transpose() << std::endl; + return & sensor_position_vehicle_; +} + +const Eigen::Matrix3s * SensorBase::getSensorRotation() const { - return & sensor_pose_vehicle_; + //std::cout << "getSensorRotation: " << sensor_rotation_vehicle_ << std::endl; + return & sensor_rotation_vehicle_; } + diff --git a/src/sensor_base.h b/src/sensor_base.h index a0dcb2d16cfc4b9d4a1727425217969cf427ffba..c822a405c21c3f9907d0773bccb2760b55c508a5 100644 --- a/src/sensor_base.h +++ b/src/sensor_base.h @@ -1,7 +1,6 @@ #ifndef SENSOR_BASE_H_ #define SENSOR_BASE_H_ - //std includes #include <iostream> @@ -13,20 +12,23 @@ class SensorBase : public NodeBase { protected: SensorType type_;//indicates sensor type. Enum defined at wolf.h - Eigen::VectorXs sensor_pose_vehicle_;//sensor pose in the vehicle frame + Eigen::Vector3s sensor_position_vehicle_;//sensor position in the vehicle frame + Eigen::Matrix3s sensor_rotation_vehicle_;//sensor rotation in the vehicle frame Eigen::VectorXs params_;//sensor intrinsic params: offsets, scale factors, sizes, ... //bool generate_prior_; //flag indicating if this sensor generates the prior or not public: - SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, const Eigen::VectorXs & _params); + SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, const Eigen::VectorXs & _params); - SensorBase(const SensorType & _tp, const Eigen::VectorXs & _pose, unsigned int _params_size); + SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, unsigned int _params_size); ~SensorBase(); const SensorType getSensorType() const; - const Eigen::VectorXs * getSensorPose() const; + const Eigen::Vector3s * getSensorPosition() const; + + const Eigen::Matrix3s * getSensorRotation() const; }; #endif diff --git a/src/sensor_gps_fix.cpp b/src/sensor_gps_fix.cpp index f12a2cd2933c3e183f2d0d8d688fa7e59abf4867..f9a52cd490e55ad95d74767b06c891aa028ef003 100644 --- a/src/sensor_gps_fix.cpp +++ b/src/sensor_gps_fix.cpp @@ -1,6 +1,6 @@ #include "sensor_gps_fix.h" -SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _sp, const double& _noise) : +SensorGPSFix::SensorGPSFix(const Eigen::Vector6s & _sp, const double& _noise) : SensorBase(GPS_FIX, _sp, Eigen::VectorXs::Constant(1,_noise)) { // diff --git a/src/sensor_gps_fix.h b/src/sensor_gps_fix.h index 65a3ae03846d0340d0c8407179346b1153db6cca..d7fe447249c6d52554073703a3c30ddf50dc0e87 100644 --- a/src/sensor_gps_fix.h +++ b/src/sensor_gps_fix.h @@ -15,7 +15,7 @@ class SensorGPSFix : public SensorBase * \param _noise noise standard deviation * **/ - SensorGPSFix(const Eigen::VectorXs & _sp, const double& _noise); + SensorGPSFix(const Eigen::Vector6s & _sp, const double& _noise); /** \brief Destructor * diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp index cd282f28130d3f0fc81cff6418500d1ec3109516..0e1e0be9d05feb444c37d22ea3c06c451791d601 100644 --- a/src/sensor_laser_2D.cpp +++ b/src/sensor_laser_2D.cpp @@ -7,7 +7,7 @@ // params_ << _angle_min, _angle_max, _angle_increment, _range_min, _range_max, _range_stdev, _time_increment, _scan_time; // } -SensorLaser2D::SensorLaser2D(const Eigen::VectorXs & _sp) : +SensorLaser2D::SensorLaser2D(const Eigen::Vector6s & _sp) : SensorBase(LIDAR, _sp, 8) { setDefaultScanParams(); diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h index 67b3db094e49504413630abbe2d23a74088653fb..c8fb24ce7d7596d730159bab756d663f0cbd43f5 100644 --- a/src/sensor_laser_2D.h +++ b/src/sensor_laser_2D.h @@ -44,7 +44,7 @@ class SensorLaser2D : public SensorBase * \param _params struct with scan parameters. See laser_scan_utils library API for reference * **/ - SensorLaser2D(const Eigen::VectorXs & _sp); + SensorLaser2D(const Eigen::Vector6s & _sp); //SensorLaser2D(const Eigen::VectorXs & _sp, const laserscanutils::ScanParams & _params); /** \brief Destructor diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp index 5bf66af15b572d34313918e926db2994b5f5b211..c395464610d05cb3d24ad620cf2353a4b7ab831c 100644 --- a/src/sensor_odom_2D.cpp +++ b/src/sensor_odom_2D.cpp @@ -1,6 +1,6 @@ #include "sensor_odom_2D.h" -SensorOdom2D::SensorOdom2D(const Eigen::VectorXs & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) : +SensorOdom2D::SensorOdom2D(const Eigen::Vector6s & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) : SensorBase(ODOM_2D, _sp, 2) { params_ << _disp_noise_factor, _rot_noise_factor; diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h index c81628ba3e4604c6a56a82427b36458371206dd2..84491a92899a88a76c9cc6b5bb7085b2fb149a64 100644 --- a/src/sensor_odom_2D.h +++ b/src/sensor_odom_2D.h @@ -16,7 +16,7 @@ class SensorOdom2D : public SensorBase * \param _rot_noise_factor rotation noise factor * **/ - SensorOdom2D(const Eigen::VectorXs & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor); + SensorOdom2D(const Eigen::Vector6s & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor); /** \brief Destructor * diff --git a/src/wolf.h b/src/wolf.h index 5583edd5aa0df28c7864e1d74d8b9ebaefa196a4..aee57e5d64369b8463533d66cfcef8b78f1eba1c 100644 --- a/src/wolf.h +++ b/src/wolf.h @@ -62,6 +62,7 @@ typedef Matrix<WolfScalar, 1, 1> Vector1s; ///< 1-vector of real typedef Matrix<WolfScalar, 2, 1> Vector2s; ///< 2-vector of real scalar_t type typedef Matrix<WolfScalar, 3, 1> Vector3s; ///< 3-vector of real scalar_t type typedef Matrix<WolfScalar, 4, 1> Vector4s; ///< 4-vector of real scalar_t type +typedef Matrix<WolfScalar, 6, 1> Vector6s; ///< 6-vector of real scalar_t type typedef Matrix<WolfScalar, Dynamic, 1> VectorXs; ///< variable size vector of real scalar_t type typedef Matrix<WolfScalar, 1, 2> RowVector2s; ///< 2-row-vector of real scalar_t type typedef Matrix<WolfScalar, 1, 3> RowVector3s; ///< 3-row-vector of real scalar_t type