Skip to content
Snippets Groups Projects
Commit 62c67c92 authored by jvallve's avatar jvallve
Browse files

test amb 2 lasers funcionant

parent 75fe7517
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 738 additions and 332 deletions
src/capture_laser_2D.cpp
+ 40
− 295
View file @ 62c67c92
......@@ -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,
......
src/capture_odom_2D.cpp
+ 2
− 0
View file @ 62c67c92
......@@ -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();
}
......
src/constraint_corner_2D_theta.h
+ 23
− 8
View file @ 62c67c92
#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;
......
src/examples/CMakeLists.txt
+ 6
− 1
View file @ 62c67c92
......@@ -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})
......
src/examples/test_ceres_2_lasers.cpp 0 → 100644
+ 622
− 0
View file @ 62c67c92
This diff is collapsed.
src/examples/test_ceres_manager.cpp
+ 2
− 2
View file @ 62c67c92
......@@ -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;
......
src/examples/test_ceres_manager_tree.cpp
+ 9
− 8
View file @ 62c67c92
......@@ -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
......
src/sensor_base.cpp
+ 20
− 7
View file @ 62c67c92
#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_;
}
src/sensor_base.h
+ 7
− 5
View file @ 62c67c92
#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
......
src/sensor_gps_fix.cpp
+ 1
− 1
View file @ 62c67c92
#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))
{
//
......
src/sensor_gps_fix.h
+ 1
− 1
View file @ 62c67c92
......@@ -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
*
......
src/sensor_laser_2D.cpp
+ 1
− 1
View file @ 62c67c92
......@@ -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();
......
src/sensor_laser_2D.h
+ 1
− 1
View file @ 62c67c92
......@@ -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
......
src/sensor_odom_2D.cpp
+ 1
− 1
View file @ 62c67c92
#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;
......
src/sensor_odom_2D.h
+ 1
− 1
View file @ 62c67c92
......@@ -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
*
......
src/wolf.h
+ 1
− 0
View file @ 62c67c92
......@@ -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
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment