diff --git a/aslam_cv/aslam_cameras/include/aslam/cameras/GridCalibrationTargetBase.hpp b/aslam_cv/aslam_cameras/include/aslam/cameras/GridCalibrationTargetBase.hpp index 9db73ea0a..fa9bf345c 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/GridCalibrationTargetBase.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/GridCalibrationTargetBase.hpp @@ -15,6 +15,7 @@ #include #include + namespace aslam { namespace cameras { @@ -78,7 +79,8 @@ class GridCalibrationTargetBase { /// in outImagePoints was observed virtual bool computeObservation(const cv::Mat & /*image*/, Eigen::MatrixXd & /*outImagePoints*/, - std::vector & /*outCornerObserved*/) const + std::vector & /*outCornerObserved*/ + ) const { SM_ASSERT_TRUE(Exception, true, "you need to implement this method for each target!"); return false; diff --git a/aslam_cv/aslam_cameras/include/aslam/cameras/PinholeProjection.hpp b/aslam_cv/aslam_cameras/include/aslam/cameras/PinholeProjection.hpp index 79eb347bd..6c0c349af 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/PinholeProjection.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/PinholeProjection.hpp @@ -11,7 +11,6 @@ #include #include #include -#include namespace aslam { namespace cameras { @@ -209,7 +208,7 @@ class PinholeProjection { int height() const { return _rv; } - + int keypointDimension() const { return KeypointDimension; } diff --git a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/OmniProjection.hpp b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/OmniProjection.hpp index 084dafde3..4c7b4333f 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/OmniProjection.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/OmniProjection.hpp @@ -744,7 +744,7 @@ bool OmniProjection::initializeIntrinsics(const std::vector::initializeIntrinsics(const std::vector::initializeIntrinsics(const std::vector MIN_CORNERS) + size_t MIN_CORNERS = 4; + if (assymetric) + MIN_CORNERS = 2; + if (count >= MIN_CORNERS) { // Resize P to fit with the count of valid points. cv::Mat C; diff --git a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp index 51f237e47..be4e9b424 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp @@ -1,4 +1,5 @@ #include + #include namespace aslam { @@ -26,7 +27,6 @@ PinholeProjection::PinholeProjection( _cv = config.getDouble("cv"); _ru = config.getInt("ru"); _rv = config.getInt("rv"); - updateTemporaries(); } @@ -36,8 +36,9 @@ PinholeProjection::PinholeProjection(double focalLengthU, double imageCenterU, double imageCenterV, int resolutionU, - int resolutionV, - distortion_t distortion) + int resolutionV, + distortion_t distortion + ) : _fu(focalLengthU), _fv(focalLengthV), _cu(imageCenterU), @@ -54,13 +55,15 @@ PinholeProjection::PinholeProjection(double focalLengthU, double imageCenterU, double imageCenterV, int resolutionU, - int resolutionV) + int resolutionV + ) : _fu(focalLengthU), _fv(focalLengthV), _cu(imageCenterU), _cv(imageCenterV), _ru(resolutionU), _rv(resolutionV) { + updateTemporaries(); } @@ -86,6 +89,7 @@ bool PinholeProjection::euclideanToKeypoint( outKeypoint[0] = p[0] * rz; outKeypoint[1] = p[1] * rz; + _distortion.distort(outKeypoint); outKeypoint[0] = _fu * outKeypoint[0] + _cu; @@ -147,7 +151,6 @@ template bool PinholeProjection::homogeneousToKeypoint( const Eigen::MatrixBase & ph, const Eigen::MatrixBase & outKeypoint) const { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC( Eigen::MatrixBase, 4); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC( @@ -210,6 +213,7 @@ bool PinholeProjection::keypointToEuclidean( kp[0] = (kp[0] - _cu) / _fu; kp[1] = (kp[1] - _cv) / _fv; + _distortion.undistort(kp); // revert distortion Eigen::MatrixBase & outPoint = const_cast ipts; double d = hypot(x1-x2, y1-y2); + if (d > r1 + r2) { // circles are separate return ipts; @@ -693,6 +698,7 @@ class PinholeHelpers { double median; size_t size = values.size(); std::sort(values.begin(), values.end()); + if (size%2 == 0) median = (values[size / 2 - 1] + values[size / 2]) / 2; else @@ -720,9 +726,11 @@ bool PinholeProjection::initializeIntrinsics(const std::vector::initializeIntrinsics(const std::vector f_guesses; for (size_t i=0; i::initializeIntrinsics(const std::vector> center(target.rows()); double radius[target.rows()]; - bool skipImage=false; + int count=0; + int MIN_TAGS = 3; + //If the grid is assymetric we never see all grid Points. Initialization can't be the same as for the symmetric case. + bool skipImage=false; + bool assymetric=false; for (size_t r=0; r circle; + for (size_t c=0; c change method to allow less observations or initialize with constant value + if (target.gridPoint(r,c)(0)==-1){ + assymetric = true; + continue; + } + if (obs.imageGridPoint(r, c, imagePoint)){ circle.push_back(cv::Point2f(imagePoint[0], imagePoint[1])); - else + count ++; + } + else if(!assymetric) //skip this image if the board view is not complete skipImage=true; + } + PinholeHelpers::fitCircle(circle, center[r](0), center[r](1), radius[r]); } + //If we see two points of the same tag we have at least one horizontal line. See at least 3 tags? 2 tags? + if(count >= MIN_TAGS*4 && assymetric) + skipImage=false; - if(skipImage) + if(skipImage){ continue; + } + for (size_t j=0; j::initializeIntrinsics(const std::vector ipts; + if( std::isinf(center[j](0)) || std::isnan(center[j](0)) || std::isinf(center[k](0)) || std::isnan(center[k](0)) ) + continue; ipts = PinholeHelpers::intersectCircles(center[j](0), center[j](1), radius[j], center[k](0), center[k](1), radius[k]); + if (ipts.size()<2) - continue; + continue; double f_guess = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI; f_guesses.push_back(f_guess); @@ -780,8 +810,8 @@ bool PinholeProjection::initializeIntrinsics(const std::vector::computeReprojectionError( for (size_t i = 0; i < obs.target()->size(); ++i) { Eigen::Vector2d y, yhat; + if (obs.imagePoint(i, y) && euclideanToKeypoint(T_camera_target * obs.target()->point(i), yhat)) { @@ -819,13 +850,14 @@ template bool PinholeProjection::estimateTransformation( const GridCalibrationTargetObservation & obs, sm::kinematics::Transformation & out_T_t_c) const { - + std::vector Ms; std::vector Ps; // Get the observed corners in the image and target frame obs.getCornersImageFrame(Ms); obs.getCornersTargetFrame(Ps); + // Convert all target corners to a fakey pinhole view. size_t count = 0; @@ -848,7 +880,8 @@ bool PinholeProjection::estimateTransformation( ++count; } } - + + Ps.resize(count); Ms.resize(count); @@ -859,9 +892,10 @@ bool PinholeProjection::estimateTransformation( if (Ps.size() < 4) return false; + - // Call the OpenCV pnp function. - cv::solvePnP(Ps, Ms, cv::Mat::eye(3, 3, CV_64F), distCoeffs, rvec, tvec); + // Call the OpenCV pnp function. + cv::solvePnP(Ps, Ms, cv::Mat::eye(3, 3, CV_64F), distCoeffs, rvec, tvec); // convert the rvec/tvec to a transformation cv::Mat C_camera_model = cv::Mat::eye(3, 3, CV_64F); @@ -878,6 +912,7 @@ bool PinholeProjection::estimateTransformation( return true; } + } // namespace cameras } // namespace aslam diff --git a/aslam_cv/aslam_cameras/src/GridCalibrationTargetObservation.cpp b/aslam_cv/aslam_cameras/src/GridCalibrationTargetObservation.cpp index 18cb6904c..f5bcb354b 100644 --- a/aslam_cv/aslam_cameras/src/GridCalibrationTargetObservation.cpp +++ b/aslam_cv/aslam_cameras/src/GridCalibrationTargetObservation.cpp @@ -33,6 +33,7 @@ void GridCalibrationTargetObservation::setTarget( _points = Eigen::MatrixXd::Zero(target->size(), 2); _success.clear(); _success.resize(target->size(), false); + } /// \brief get all (observed) corners in image coordinates diff --git a/aslam_cv/aslam_cameras/src/GridDetector.cpp b/aslam_cv/aslam_cameras/src/GridDetector.cpp index 93c2b237e..e1b35ac0a 100644 --- a/aslam_cv/aslam_cameras/src/GridDetector.cpp +++ b/aslam_cv/aslam_cameras/src/GridDetector.cpp @@ -92,10 +92,10 @@ bool GridDetector::findTarget(const cv::Mat & image, bool GridDetector::findTargetNoTransformation(const cv::Mat & image, const aslam::Time & stamp, GridCalibrationTargetObservation & outObservation) const { bool success = false; - // Extract the calibration target corner points Eigen::MatrixXd cornerPoints; std::vector validCorners; + success = _target->computeObservation(image, cornerPoints, validCorners); // Set the image, target, and timestamp regardless of success. @@ -105,10 +105,13 @@ bool GridDetector::findTargetNoTransformation(const cv::Mat & image, const aslam // Set the observed corners in the observation for (int i = 0; i < cornerPoints.rows(); i++) { - if (validCorners[i]) + + //if Success we have validCorners and updateImagePoint runs + + if (validCorners[i]){ outObservation.updateImagePoint(i, cornerPoints.row(i).transpose()); + } } - return success; } diff --git a/aslam_cv/aslam_cameras_april/CMakeLists.txt b/aslam_cv/aslam_cameras_april/CMakeLists.txt index 314f953d2..d7df51910 100644 --- a/aslam_cv/aslam_cameras_april/CMakeLists.txt +++ b/aslam_cv/aslam_cameras_april/CMakeLists.txt @@ -12,6 +12,7 @@ include_directories(${Eigen_INCLUDE_DIRS}) cs_add_library(${PROJECT_NAME} src/GridCalibrationTargetAprilgrid.cpp + src/GridCalibrationTargetAssymetricAprilgrid.cpp ) find_package(Boost REQUIRED COMPONENTS serialization system) diff --git a/aslam_cv/aslam_cameras_april/include/aslam/cameras/GridCalibrationTargetAprilgrid.hpp b/aslam_cv/aslam_cameras_april/include/aslam/cameras/GridCalibrationTargetAprilgrid.hpp index 872d316e9..97c632649 100644 --- a/aslam_cv/aslam_cameras_april/include/aslam/cameras/GridCalibrationTargetAprilgrid.hpp +++ b/aslam_cv/aslam_cameras_april/include/aslam/cameras/GridCalibrationTargetAprilgrid.hpp @@ -15,7 +15,8 @@ //#include "apriltags/Tag25h7.h" //#include "apriltags/Tag25h9.h" //#include "apriltags/Tag36h9.h" -#include "apriltags/Tag36h11.h" +//#include "apriltags/Tag36h11.h" +#include "apriltags/AllTags.h" namespace aslam { namespace cameras { diff --git a/aslam_cv/aslam_cameras_april/include/aslam/cameras/GridCalibrationTargetAssymetricAprilgrid.hpp b/aslam_cv/aslam_cameras_april/include/aslam/cameras/GridCalibrationTargetAssymetricAprilgrid.hpp new file mode 100644 index 000000000..2c697635d --- /dev/null +++ b/aslam_cv/aslam_cameras_april/include/aslam/cameras/GridCalibrationTargetAssymetricAprilgrid.hpp @@ -0,0 +1,136 @@ +#ifndef ASLAM_GRID_CALIBRATION_TARGET_ASSYMETRIC_APRILGRID_HPP +#define ASLAM_GRID_CALIBRATION_TARGET_ASSYMETRIC_APRILGRID_HPP + +#include +#include +#include "apriltags/AllTags.h" +#include "apriltags/TagDetector.h" +#include +#include +#include + +#define TAGCODES AprilTags::tagCodes25h4 + +namespace aslam { +namespace cameras { + +class GridCalibrationTargetAssymetricAprilgrid : public GridCalibrationTargetBase { + public: + SM_DEFINE_EXCEPTION(Exception, std::runtime_error); + + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + struct TargetPoint { + TargetPoint(): + x(-1), + y(-1), + size(0), + row{}, + col{}, + id(-1) {}; + + float x,y; + unsigned int size; + std::array row, col; + int id; + + /// \brief Serialization support + enum {CLASS_SERIALIZATION_VERSION = 1}; + BOOST_SERIALIZATION_SPLIT_MEMBER(); + template + void save(Archive & ar, const unsigned int /*version*/) const + { + ar << BOOST_SERIALIZATION_NVP(x); + ar << BOOST_SERIALIZATION_NVP(y); + ar << BOOST_SERIALIZATION_NVP(size); + ar << BOOST_SERIALIZATION_NVP(row); + ar << BOOST_SERIALIZATION_NVP(col); + ar << BOOST_SERIALIZATION_NVP(id); + } + template + void load(Archive & ar, const unsigned int /*version*/) + { + ar >> BOOST_SERIALIZATION_NVP(x); + ar >> BOOST_SERIALIZATION_NVP(y); + ar >> BOOST_SERIALIZATION_NVP(size); + ar >> BOOST_SERIALIZATION_NVP(row); + ar >> BOOST_SERIALIZATION_NVP(col); + ar >> BOOST_SERIALIZATION_NVP(id); + } + + +}; + + + /// \brief initialize based on checkerboard geometry + GridCalibrationTargetAssymetricAprilgrid(const std::vector &targetPoints, + const GridCalibrationTargetAprilgrid::AprilgridOptions &options = GridCalibrationTargetAprilgrid::AprilgridOptions()); + + GridCalibrationTargetAssymetricAprilgrid(const boost::python::list& vector_list, + const GridCalibrationTargetAprilgrid::AprilgridOptions &options= GridCalibrationTargetAprilgrid::AprilgridOptions()); + + virtual ~GridCalibrationTargetAssymetricAprilgrid() {}; + + /// \brief extract the calibration target points from an image and write to an observation + bool computeObservation(const cv::Mat & image, + Eigen::MatrixXd & outImagePoints, + std::vector &outCornerObserved) const; + + private: + void initialize(); + void createGridPoints(); + void targetPoints2Grid(); + std::vector _targetPoints; + GridCalibrationTargetAprilgrid::AprilgridOptions _options; + + // create a detector instance + AprilTags::TagCodes _tagCodes; + boost::shared_ptr _tagDetector; + /////////////////////////////////////////////////// + // Serialization support + /////////////////////////////////////////////////// + public: + enum {CLASS_SERIALIZATION_VERSION = 1}; + BOOST_SERIALIZATION_SPLIT_MEMBER() + + //serialization ctor + GridCalibrationTargetAssymetricAprilgrid(); + + protected: + friend class boost::serialization::access; + + template + void save(Archive & ar, const unsigned int /* version */) const { + boost::serialization::void_cast_register( + static_cast(NULL), + static_cast(NULL)); + ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(GridCalibrationTargetBase); + ar << BOOST_SERIALIZATION_NVP(_targetPoints); + ar << BOOST_SERIALIZATION_NVP(_options); + + } + template + void load(Archive & ar, const unsigned int /* version */) { + boost::serialization::void_cast_register( + static_cast(NULL), + static_cast(NULL)); + ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(GridCalibrationTargetBase); + ar >> BOOST_SERIALIZATION_NVP(_targetPoints); + ar >> BOOST_SERIALIZATION_NVP(_options); + + initialize(); + } +}; + + +} // namespace cameras +} // namespace aslam + + +SM_BOOST_CLASS_VERSION(aslam::cameras::GridCalibrationTargetAssymetricAprilgrid); +SM_BOOST_CLASS_VERSION(aslam::cameras::GridCalibrationTargetAssymetricAprilgrid::TargetPoint); +BOOST_CLASS_EXPORT_KEY(aslam::cameras::GridCalibrationTargetAssymetricAprilgrid) + +#endif /* ASLAM_GRID_CALIBRATION_TARGET_ASSYMETRIC_APRILGRID_HPP */ + diff --git a/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAssymetricAprilgrid.cpp b/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAssymetricAprilgrid.cpp new file mode 100644 index 000000000..976d7fcec --- /dev/null +++ b/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAssymetricAprilgrid.cpp @@ -0,0 +1,438 @@ +#include +#include +#include +#include +#include +#include +#include + +namespace aslam { +namespace cameras { + +/// \brief Construct an Aprilgrid calibration target +/// tagRows: number of tags in y-dir (gridRows = 2*tagRows) +/// tagCols: number of tags in x-dir (gridCols = 2*tagCols) +/// tagSize: size of a tag [m] +/// tagSpacing: space between tags (in tagSpacing [m] = tagSpacing*tagSize) +/// +/// corner ordering in _points : +/// 12-----13 14-----15 +/// | TAG 3 | | TAG 4 | +/// 8-------9 10-----11 +/// 4-------5 6-------7 +/// y | TAG 1 | | TAG 2 | +/// ^ 0-------1 2-------3 +/// |-->x +GridCalibrationTargetAssymetricAprilgrid::GridCalibrationTargetAssymetricAprilgrid(const std::vector &targetPoints, + const GridCalibrationTargetAprilgrid::AprilgridOptions &options) + : GridCalibrationTargetBase(), + _targetPoints(targetPoints), + _options(options), + _tagCodes(TAGCODES) + { + + //Assign cols and rows of the grid based on position + targetPoints2Grid(); + + + _points.setConstant(size(), 3,-1); + + //initialize a normal grid (checkerboard and circlegrids) + createGridPoints(); + + //start the output window if requested + initialize(); + + +} +GridCalibrationTargetAssymetricAprilgrid::GridCalibrationTargetAssymetricAprilgrid(const boost::python::list& vector_list,const GridCalibrationTargetAprilgrid::AprilgridOptions &options): + GridCalibrationTargetBase(), + _options(options), + _tagCodes(TAGCODES){ + + std::vector targetPoints; + + for (int i = 0; i < boost::python::len(vector_list); ++i) + { + targetPoints.push_back((boost::python::extract(vector_list[i]))); + } + + _targetPoints =targetPoints; + + targetPoints2Grid(); + + _points.setConstant(size(), 3,-1); + + //initialize a normal grid (checkerboard and circlegrids) + createGridPoints(); + + //start the output window if requested + initialize(); + + } +GridCalibrationTargetAssymetricAprilgrid::GridCalibrationTargetAssymetricAprilgrid(): + _tagCodes(TAGCODES) + {} + + + + void GridCalibrationTargetAssymetricAprilgrid::targetPoints2Grid(){ + + std::vector rows; + std::vector cols; + std::vector::iterator it_rows; + std::vector::iterator it_cols; + + TargetPoint *tag; + + for (size_t i=0; i< _targetPoints.size(); i++){ + tag = &_targetPoints.at(i); + tag->id = i; //already ordered by id in familycodes + it_rows = find (rows.begin(), rows.end(), tag->y); + it_cols = find (cols.begin(), cols.end(), tag->x); + + if (it_rows==rows.end()) + rows.push_back(tag->y); + if(it_cols ==cols.end()) + cols.push_back(tag->x); + + //ou row col based on size + it_rows = find (rows.begin(), rows.end(), tag->y + tag->size); + it_cols = find (cols.begin(), cols.end(), tag->x + tag->size); + + if (it_rows==rows.end()) + rows.push_back(tag->y + tag->size); + if(it_cols ==cols.end()) + cols.push_back(tag->x + tag->size); + } + + std::sort(rows.begin(),rows.end()); + std::sort(cols.begin(),cols.end()); + + for (size_t i=0; i< _targetPoints.size(); i++){ + tag = &_targetPoints.at(i); + + it_rows = find (rows.begin(), rows.end(), tag->y); + it_cols = find (cols.begin(), cols.end(), tag->x); + + tag->row[0] = std::distance (rows.begin(),it_rows); + tag->col[0] = std::distance (cols.begin(),it_cols); + + it_rows = find (rows.begin(), rows.end(), tag->y + tag->size); + it_cols = find (cols.begin(), cols.end(), tag->x + tag->size); + + tag->row[1] =std::distance (rows.begin(),it_rows); + tag->col[1] =std::distance (cols.begin(),it_cols); + + } + + _rows=rows.size(); + _cols=cols.size(); + } +void GridCalibrationTargetAssymetricAprilgrid::initialize() +{ + if (_options.showExtractionVideo) { + cv::namedWindow("Aprilgrid: Tag detection"); + cv::namedWindow("Aprilgrid: Tag corners"); + cvStartWindowThread(); + } + + //create the tag detector + _tagDetector = boost::make_shared(_tagCodes, _options.blackTagBorder); +} + +/// \brief initialize an april grid +/// point ordering: (e.g. 2x2 grid) +/// 12-----13 14-----15 +/// | TAG 3 | | TAG 4 | +/// 8-------9 10-----11 +/// 4-------5 6-------7 2 3 +/// y | TAG 1 | | TAG 2 | +/// ^ 0-------1 2-------3 0-----1 +/// |-->x +void GridCalibrationTargetAssymetricAprilgrid::createGridPoints() { + + int index; + + for (unsigned i = 0; i < _targetPoints.size(); i++) { + TargetPoint *tag = &_targetPoints.at(i); + + for (unsigned k1=0; k1<2; k1++) + for (unsigned k2=0; k2<2; k2 ++){ + index = tag->row[k1]*_cols + tag->col[k2]; + Eigen::Matrix point; + point(0) = tag->x*0.001 + k2*tag->size*0.001; //mm -> m + point(1) = tag->y*0.001 + k1*tag->size*0.001; + point(2) = 0.0; + + _points.row(index) = point; + } + } + + +} + +/// \brief extract the calibration target points from an image and write to an observation + bool GridCalibrationTargetAssymetricAprilgrid::computeObservation( + const cv::Mat & image, Eigen::MatrixXd & outImagePoints, + std::vector &outCornerObserved ) const { + + bool success = true; + + // detect the tags + std::vector detections = _tagDetector->extractTags(image); + + /* handle the case in which a tag is identified but not all tag + * corners are in the image (all data bits in image but border + * outside). tagCorners should still be okay as apriltag-lib + * extrapolates them, only the subpix refinement will fail + */ + + //min. distance [px] of tag corners from image border (tag is not used if violated) + std::vector::iterator iter = detections.begin(); + + for (iter = detections.begin(); iter != detections.end();) { + // check all four corners for violation + bool remove = false; + + for (int j = 0; j < 4; j++) { + remove |= iter->p[j].first < _options.minBorderDistance; + remove |= iter->p[j].first > (float) (image.cols) - _options.minBorderDistance; //width + remove |= iter->p[j].second < _options.minBorderDistance; + remove |= iter->p[j].second > (float) (image.rows) - _options.minBorderDistance; //height + } + + //also remove tags that are flagged as bad + if (iter->good != 1) + remove |= true; + + //also remove if the tag ID is out-of-range for this grid (faulty detection) + if (iter->id >= (int) size() / 4) + remove |= true; + + // delete flagged tags + if (remove) { + SM_DEBUG_STREAM("Tag with ID " << iter->id << " is only partially in image (corners outside) and will be removed from the TargetObservation.\n"); + + // delete the tag and advance in list + iter = detections.erase(iter); + } else { + //advance in list + ++iter; + } + } + + + if (detections.size() < _options.minTagsForValidObs) { + success = false; + + //immediate exit if we dont need to show video for debugging... + //if video is shown, exit after drawing video... + if (!_options.showExtractionVideo) + return success; + } + + //sort detections by tagId + std::sort(detections.begin(), detections.end(), + AprilTags::TagDetection::sortByIdCompare); + + // check for duplicate tagIds (--> if found: wild Apriltags in image not belonging to calibration target) + // (only if we have more than 1 tag...) + if (detections.size() > 1) { + for (unsigned i = 0; i < detections.size() - 1; i++) + if (detections[i].id == detections[i + 1].id) { + //show the duplicate tags in the image + cv::destroyAllWindows(); + cv::namedWindow("Wild Apriltag detected. Hide them!"); + cvStartWindowThread(); + + cv::Mat imageCopy = image.clone(); + cv::cvtColor(imageCopy, imageCopy, CV_GRAY2RGB); + + //mark all duplicate tags in image + for (int j = 0; i < detections.size() - 1; i++) { + if (detections[j].id == detections[j + 1].id) { + detections[j].draw(imageCopy); + detections[j + 1].draw(imageCopy); + } + } + + cv::putText(imageCopy, "Duplicate Apriltags detected. Hide them.", + cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8, + CV_RGB(255,0,0), 2, 8, false); + cv::putText(imageCopy, "Press enter to exit...", cv::Point(50, 80), + CV_FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255,0,0), 2, 8, false); + cv::imshow("Duplicate Apriltags detected. Hide them", imageCopy); // OpenCV call + + // and exit + SM_FATAL_STREAM("\n[ERROR]: Found apriltag not belonging to calibration board. Check the image for the tag and hide it.\n"); + + cv::waitKey(); + // close the window + cv::destroyAllWindows(); + + } + } + + // convert corners to cv::Mat (4 consecutive corners form one tag) + /// point ordering here + /// 11-----10 15-----14 + /// | TAG 2 | | TAG 3 | + /// 8-------9 12-----13 + /// 3-------2 7-------6 + /// y | TAG 0 | | TAG 1 | + /// ^ 0-------1 4-------5 + /// |-->x + cv::Mat tagCorners(4 * detections.size(), 2, CV_32F); + + for (unsigned i = 0; i < detections.size(); i++) { + for (unsigned j = 0; j < 4; j++) { + tagCorners.at(4 * i + j, 0) = detections[i].p[j].first; + tagCorners.at(4 * i + j, 1) = detections[i].p[j].second; + } + } + + //store a copy of the corner list before subpix refinement + cv::Mat tagCornersRaw = tagCorners.clone(); + + //optional subpixel refinement on all tag corners (four corners each tag) + if (_options.doSubpixRefinement && success) + cv::cornerSubPix( + image, tagCorners, cv::Size(2, 2), cv::Size(-1, -1), + cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); + + if (_options.showExtractionVideo) { + //image with refined (blue) and raw corners (red) + cv::Mat imageCopy1 = image.clone(); + cv::cvtColor(imageCopy1, imageCopy1, CV_GRAY2RGB); + for (unsigned i = 0; i < detections.size(); i++) + for (unsigned j = 0; j < 4; j++) { + //raw apriltag corners + //cv::circle(imageCopy1, cv::Point2f(detections[i].p[j].first, detections[i].p[j].second), 2, CV_RGB(255,0,0), 1); + + //subpixel refined corners + cv::circle( + imageCopy1, + cv::Point2f(tagCorners.at(4 * i + j, 0), + tagCorners.at(4 * i + j, 1)), + 3, CV_RGB(0,0,255), 1); + + if (!success) + cv::putText(imageCopy1, "Detection failed! (frame not used)", + cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8, + CV_RGB(255,0,0), 3, 8, false); + } + + cv::imshow("Aprilgrid: Tag corners", imageCopy1); // OpenCV call + cv::waitKey(1); + + /* copy image for modification */ + cv::Mat imageCopy2 = image.clone(); + cv::cvtColor(imageCopy2, imageCopy2, CV_GRAY2RGB); + /* highlight detected tags in image */ + for (unsigned i = 0; i < detections.size(); i++) { + detections[i].draw(imageCopy2); + + if (!success) + cv::putText(imageCopy2, "Detection failed! (frame not used)", + cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8, + CV_RGB(255,0,0), 3, 8, false); + } + + cv::imshow("Aprilgrid: Tag detection", imageCopy2); // OpenCV call + cv::waitKey(1); + + //if success is false exit here (delayed exit if _options.showExtractionVideo=true for debugging) + if (!success) + return success; + } + + //insert the observed points into the correct location of the grid point array + /// point ordering + /// 12-----13 14-----15 + /// | TAG 2 | | TAG 3 | + /// 8-------9 10-----11 + /// 4-------5 6-------7 + /// y | TAG 0 | | TAG 1 | + /// ^ 0-------1 2-------3 + /// |-->x + + outCornerObserved.resize(size(), false); + outImagePoints.resize(size(), 2); + //int tagId_pos[11]={0,8,14,2,16,10,4,18,6,12,20}; + std::vector::const_iterator it; + for (unsigned int i = 0; i < detections.size(); i++) { + it = _targetPoints.begin(); + + + // get the tag id + unsigned int tagId = detections[i].id; + + // calculate the grid idx for all four tag corners given the tagId and cols + + //unsigned pos= tagId_pos[tagId]; + //unsigned int baseId = (int) (pos / (_cols / 2)) * _cols * 2 + // + (pos % (_cols / 2)) * 2; + it = std::find_if(_targetPoints.begin(), _targetPoints.end(), + boost::bind(&TargetPoint::id, _1) == tagId); + unsigned int pIdx[] = {0,0,0,0}; + + if(it != _targetPoints.end()){ + pIdx[0] =it->row[0]*_cols + it->col[0]; + pIdx[1]= it->row[0]*_cols + it->col[1]; + pIdx[2] = it->row[1]*_cols + it->col[1]; + pIdx[3] =it->row[1]*_cols + it->col[0] ; + } + + + + // unsigned int pIdx[] = { baseId, baseId + 1, baseId + (unsigned int) _cols + // + 1, baseId + (unsigned int) _cols }; + + // add four points per tag + for (int j = 0; j < 4; j++) { + //refined corners + double corner_x = tagCorners.row(4 * i + j).at(0); + double corner_y = tagCorners.row(4 * i + j).at(1); + + //raw corners + double cornerRaw_x = tagCornersRaw.row(4 * i + j).at(0); + double cornerRaw_y = tagCornersRaw.row(4 * i + j).at(1); + + + //only add point if the displacement in the subpixel refinement is below a given threshold + double subpix_displacement_squarred = (corner_x - cornerRaw_x) + * (corner_x - cornerRaw_x) + + (corner_y - cornerRaw_y) * (corner_y - cornerRaw_y); + + //add all points, but only set active if the point has not moved to far in the subpix refinement + outImagePoints.row(pIdx[j]) = Eigen::Matrix(corner_x, + corner_y); + + if (subpix_displacement_squarred <= _options.maxSubpixDisplacement2) { + outCornerObserved[pIdx[j]] = true; + } else { + SM_DEBUG_STREAM("Subpix refinement failed for point: " << pIdx[j] << " with displacement: " << sqrt(subpix_displacement_squarred) << "(point removed) \n"); + outCornerObserved[pIdx[j]] = false; + for (int w = 0; w < 4; w++) { + outCornerObserved[pIdx[w]]=false; + } + break; + } + + + } + } + + //succesful observation + return success; +} + +} // namespace cameras +} // namespace aslam + +//export explicit instantions for all included archives +#include +#include +BOOST_CLASS_EXPORT_IMPLEMENT(aslam::cameras::GridCalibrationTargetAssymetricAprilgrid); +BOOST_CLASS_EXPORT_IMPLEMENT(aslam::cameras::GridCalibrationTargetAssymetricAprilgrid::TargetPoint); diff --git a/aslam_cv/aslam_cameras_april/src/module.cpp b/aslam_cv/aslam_cameras_april/src/module.cpp index 660d2786f..073024ea8 100644 --- a/aslam_cv/aslam_cameras_april/src/module.cpp +++ b/aslam_cv/aslam_cameras_april/src/module.cpp @@ -1,8 +1,11 @@ // It is extremely important to use this header // if you are using the numpy_eigen interface #include +#include + #include #include +#include BOOST_PYTHON_MODULE(libaslam_cameras_april_python) { @@ -27,4 +30,28 @@ BOOST_PYTHON_MODULE(libaslam_cameras_april_python) "GridCalibrationTargetAprilgrid(size_t tagRows, size_t tagCols, double tagSize, double tagSpacing)")) .def(init<>("Do not use the default constructor. It is only necessary for the pickle interface")) .def_pickle(sm::python::pickle_suite()); + + class_("TargetPoint", init<>()) + .def_readwrite("x", &GridCalibrationTargetAssymetricAprilgrid::TargetPoint::x) + .def_readwrite("y", &GridCalibrationTargetAssymetricAprilgrid::TargetPoint::y) + .def_readwrite("size", &GridCalibrationTargetAssymetricAprilgrid::TargetPoint::size) + .def_readwrite("row", &GridCalibrationTargetAssymetricAprilgrid::TargetPoint::row) + .def_readwrite("col", &GridCalibrationTargetAssymetricAprilgrid::TargetPoint::col) + .def_readwrite("id", &GridCalibrationTargetAssymetricAprilgrid::TargetPoint::id) + .def_pickle(sm::python::pickle_suite()); + + + class_, + boost::shared_ptr, boost::noncopyable>( + "GridCalibrationTargetAssymetricAprilgrid", + init,GridCalibrationTargetAprilgrid::AprilgridOptions>( + "GridCalibrationTargetAssymetricAprilgrid(std::vector targetPoints,AprilgridOptions options)")) + .def(init>( + "GridCalibrationTargetAssymetricAprilgrid(std::vector targetPoints)")) + .def(init( + "GridCalibrationTargetAssymetricAprilgrid(boost::python::list vector_list, GridCalibrationTargetAprilgrid::AprilgridOptions options)")) + .def(init( + "GridCalibrationTargetAssymetricAprilgrid(boost::python::list vector_list)")) + .def(init<>("Do not use the default constructor. It is only necessary for the pickle interface")) + .def_pickle(sm::python::pickle_suite()); } diff --git a/aslam_cv/aslam_cv_python/src/GridCalibration.cpp b/aslam_cv/aslam_cv_python/src/GridCalibration.cpp index 6b686e761..d47cda504 100644 --- a/aslam_cv/aslam_cv_python/src/GridCalibration.cpp +++ b/aslam_cv/aslam_cv_python/src/GridCalibration.cpp @@ -129,6 +129,7 @@ boost::python::tuple imageGridPoint( Eigen::Vector2d p = Eigen::Vector2d::Zero(); bool success = frame->imageGridPoint(r, c, p); return boost::python::make_tuple(success, p); + } /// \brief get all corners in target coordinates (order matches getCornersImageFrame) @@ -200,7 +201,7 @@ Eigen::VectorXi getCornersIdx( return imageCornersEigen; } - + void exportGridCalibration() { using namespace boost::python; diff --git a/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp b/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp index 57d177301..7de0f5b46 100644 --- a/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp +++ b/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp @@ -444,7 +444,7 @@ namespace aslam { Eigen::Matrix4d TransformationTimeOffsetExpressionNode::toTransformationMatrixImplementation() { - SM_ASSERT_GE_LT(aslam::Exception, _time.toScalar(), _bufferTmin, _bufferTmax, "Spline Coefficient Buffer Exceeded. Set larger buffer margins!"); + SM_ASSERT_GE_LT(aslam::Exception, _time.toScalar(), _bufferTmin, _bufferTmax, "Spline Coefficient Buffer Exceeded. Set larger buffer margins!"); return _spline->spline().transformation(_time.toScalar()); } diff --git a/aslam_offline_calibration/ethz_apriltag2/include/apriltags/AllTags.h b/aslam_offline_calibration/ethz_apriltag2/include/apriltags/AllTags.h new file mode 100644 index 000000000..55ffa1b4a --- /dev/null +++ b/aslam_offline_calibration/ethz_apriltag2/include/apriltags/AllTags.h @@ -0,0 +1,5 @@ +#include "apriltags/Tag16h5.h" +#include "apriltags/Tag25h7.h" +#include "apriltags/Tag25h9.h" +#include "apriltags/Tag36h9.h" +#include "apriltags/Tag36h11.h" diff --git a/aslam_offline_calibration/kalibr/python/kalibr_bagextractor b/aslam_offline_calibration/kalibr/python/kalibr_bagextractor index b1add47df..e016b4012 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_bagextractor +++ b/aslam_offline_calibration/kalibr/python/kalibr_bagextractor @@ -2,7 +2,7 @@ print "importing libraries" import kalibr_common as kc -import cv +#import cv import cv2 import csv import os @@ -54,7 +54,7 @@ if parsed.image_topics is not None: for timestamp, image in dataset: params = list() - params.append(cv.CV_IMWRITE_PNG_COMPRESSION) + params.append(cv2.CV_IMWRITE_PNG_COMPRESSION) params.append(0) #0: loss-less filename = "{0}{1:09d}.png".format(timestamp.secs, timestamp.nsecs) cv2.imwrite( "{0}/cam{1}/{2}".format(parsed.output_folder, cidx, filename), image, params ) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras index b3d7a762d..9d794142c 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras @@ -76,12 +76,12 @@ def parseArgs(): parser = KalibrArgParser(description='Calibrate the intrinsics and extrinsics of a camera system with non-shared overlapping field of view.', usage=usage) parser.add_argument('--models', nargs='+', dest='models', help='The camera model {0} to estimate'.format(cameraModels.keys()), required=True) - + groupSource = parser.add_argument_group('Data source') groupSource.add_argument('--bag', dest='bagfile', help='The bag file with the data') groupSource.add_argument('--topics', nargs='+', dest='topics', help='The list of image topics', required=True) groupSource.add_argument('--bag-from-to', metavar='bag_from_to', type=float, nargs=2, help='Use the bag data starting from up to this time [s]') - + groupTarget = parser.add_argument_group('Calibration target configuration') groupTarget.add_argument('--target', dest='targetYaml', help='Calibration target configuration as yaml file', required=True) @@ -105,7 +105,7 @@ def parseArgs(): outputSettings.add_argument('--show-extraction', action='store_true', dest='showextraction', help='Show the calibration target extraction. (disables plots)') outputSettings.add_argument('--plot', action='store_true', dest='plot', help='Plot during calibration (this could be slow).') outputSettings.add_argument('--dont-show-report', action='store_true', dest='dontShowReport', help='Do not show the report on screen after calibration.') - + #print help if no argument is specified if len(sys.argv)==1: parser.print_help() @@ -153,27 +153,28 @@ def main(): numCams = len(parsed.topics) obsdb = kcc.ObservationDatabase(parsed.max_delta_approxsync) - + for cam_id in range(0, numCams): topic = parsed.topics[cam_id] modelName = parsed.models[cam_id] print "Initializing cam{0}:".format(cam_id) print "\tCamera model:\t {0}".format(modelName) - + if modelName in cameraModels: #open dataset dataset = initBagDataset(parsed.bagfile, topic, parsed.bag_from_to) #create camera cameraModel = cameraModels[modelName] - cam = kcc.CameraGeometry(cameraModel, targetConfig, dataset, verbose=(parsed.verbose or parsed.showextraction)) + + cam = kcc.CameraGeometry(cameraModel, targetConfig, dataset,verbose=(parsed.verbose or parsed.showextraction)) #extract the targets multithreading = not (parsed.verbose or parsed.showextraction) observations = kc.extractCornersFromDataset(cam.dataset, cam.ctarget.detector, multithreading=multithreading, clearImages=False, noTransformation=True) - + #populate the database for obs in observations: obsdb.addObservation(cam_id, obs) @@ -219,15 +220,14 @@ def main(): for baseline_idx, baseline in enumerate(baseline_guesses): print "initialized baseline between cam{0} and cam{1} to:".format(baseline_idx, baseline_idx+1) - print baseline.T() - + for cam_idx, cam in enumerate(cameraList): print "initialized cam{0} to:".format(cam_idx) print "\t projection cam{0}: {1}".format(cam_idx, cam.geometry.projection().getParameters().flatten()) print "\t distortion cam{0}: {1}".format(cam_idx, cam.geometry.projection().distortion().getParameters().flatten()) - print "initializing calibrator" + calibrator = kcc.CameraCalibration(cameraList, baseline_guesses, verbose=parsed.verbose, useBlakeZissermanMest=parsed.doBlakeZisserman) options = calibrator.estimator.getOptions() options.infoGainDelta = parsed.miTol @@ -246,6 +246,7 @@ def main(): verbose = parsed.verbose doPlot = parsed.plot + if doPlot: print "Plotting during calibration. Things may be very slow (but you might learn something)." @@ -258,17 +259,17 @@ def main(): print "starting calibration..." numViews = len(timestamps) progress = sm.Progress2(numViews); progress.sample() + for view_id, timestamp in enumerate(timestamps): - #add new batch problem - obs_tuple = obsdb.getAllObsAtTimestamp(timestamp) + obs_tuple = obsdb.getAllObsAtTimestamp(timestamp) est_baselines = list() + for bidx, baseline in enumerate(calibrator.baselines): est_baselines.append( sm.Transformation(baseline.T()) ) T_tc_guess = graph.getTargetPoseGuess(timestamp, cameraList, est_baselines) - + success = calibrator.addTargetView(obs_tuple, T_tc_guess) - #display process if (verbose or (view_id % 25) == 0) and calibrator.estimator.getNumBatches()>0 and view_id>1: print @@ -334,6 +335,7 @@ def main(): #select corners to remove cornerRemovalList=list() for pidx, reproj in enumerate(rerrs[batch_id]): + if (not np.all(reproj==np.array([None,None]))) and (abs(reproj[0]) > se_threshold[0] or abs(reproj[1]) > se_threshold[1]): cornerRemovalList.append(pidx) @@ -395,9 +397,9 @@ def main(): kcc.printParameters(calibrator) if parsed.verbose and len(calibrator.baselines)>1: - f=pl.figure(100006) - kcc.plotCameraRig(calibrator.baselines, fno=f.number, clearFigure=False) - pl.show() + f=pl.figure(100006) + kcc.plotCameraRig(calibrator.baselines, fno=f.number, clearFigure=False) + pl.show() #write to file bagtag = parsed.bagfile.translate(None, "<>:/\|?*").replace('.bag', '', 1) @@ -431,6 +433,7 @@ def main(): for cam_id, cam in enumerate(cameraList): print "Reinitialize the intrinsics for camera {0}".format(cam_id) observations = obsdb.getAllObsCam(cam_id) + if not cam.initGeometryFromObservations(observations): raise RuntimeError("Could not re-initialize the intrinsics for camera with topic: {0}".format(topic)) @@ -447,3 +450,4 @@ if __name__ == "__main__": # except Exception,e: # sm.logError("Exception: {0}".format(e)) # sys.exit(-1) + diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras index ccf9aa980..a785fc6a5 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras @@ -12,7 +12,7 @@ import kalibr_common as kc import kalibr_camera_calibration as kcc import kalibr_rs_camera_calibration as rscc -import cv +#import cv import cv2 import os import numpy as np diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py index 246cb4828..67b928aca 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py @@ -28,13 +28,12 @@ class OptimizationDiverged(Exception): pass class CameraGeometry(object): - def __init__(self, cameraModel, targetConfig, dataset, geometry=None, verbose=False): + def __init__(self, cameraModel, targetConfig, dataset, verbose=False): self.dataset = dataset self.model = cameraModel - if geometry is None: - self.geometry = cameraModel.geometry() - + self.geometry = cameraModel.geometry() + if not type(self.geometry) == cameraModel.geometry: raise RuntimeError("The type of geometry passed in \"%s\" does not match the model type \"%s\"" % (type(geometry),type(cameraModel.geometry))) @@ -56,7 +55,7 @@ def initGeometryFromObservations(self, observations): success = self.geometry.initializeIntrinsics(observations) if not success: sm.logError("initialization of focal length for cam with topic {0} failed ".format(self.dataset.topic)) - + #in case of an omni model, first optimize over intrinsics only #(--> catch most of the distortion with the projection model) if self.model == acvb.DistortedOmni: @@ -115,6 +114,22 @@ def __init__(self, targetConfig, cameraGeometry, showCorners=False, showReproj=F targetParams['tagSize'], targetParams['tagSpacing'], options) + elif targetType == 'assymetric_aprilgrid': + options = acv_april.AprilgridOptions() + options.minTagsForValidObs = 1 + options.showExtractionVideo = showCorners + options.maxSubpixDisplacement2 = 2 + #options.doSubpixRefinement = False + vectorTags =[] + for tag in targetParams['tags']: + structTag = acv_april.TargetPoint() + structTag.x = tag["pos"][0] + structTag.y = tag["pos"][1] + structTag.size =tag["size"] + vectorTags.append(structTag) + + self.grid = acv_april.GridCalibrationTargetAssymetricAprilgrid(vectorTags, + options) else: RuntimeError('Unknown calibration target type!') @@ -156,7 +171,7 @@ def fromTargetViewObservations(cls, cameras, target, baselines, T_tc_guess, rig_ # 1. Create a design variable for this pose T_target_camera = T_tc_guess - + rval.dv_T_target_camera = aopt.TransformationDv(T_target_camera) for i in range(0, rval.dv_T_target_camera.numDesignVariables()): rval.addDesignVariable( rval.dv_T_target_camera.getDesignVariable(i), TRANSFORMATION_GROUP_ID) @@ -191,6 +206,7 @@ def fromTargetViewObservations(cls, cameras, target, baselines, T_tc_guess, rig_ #build baseline chain (target->cam0->baselines->camN) T_cam0_target = rval.dv_T_target_camera.expression.inverse() T_camN_calib = T_cam0_target + for idx in range(0, cam_id): T_camN_calib = baselines[idx].toExpression() * T_camN_calib @@ -368,12 +384,10 @@ def getReprojectionErrorStatistics(all_rerrs): if view_rerrs is not None: #if cam sees target in this view for rerr in view_rerrs: if not (rerr==np.array([None,None])).all(): #if corner was observed - rerr_matrix.append(rerr) - + rerr_matrix.append(rerr) rerr_matrix = np.array(rerr_matrix) gc.enable() - mean = np.mean(rerr_matrix, 0, dtype=np.float) std = np.std(rerr_matrix, 0, dtype=np.float) @@ -562,27 +576,25 @@ def recoverCovariance(cself): #split the variance for baselines baseline_cov = est_stds[0:6*(numCams-1)] std_baselines = np.array(baseline_cov).reshape(numCams-1,6).tolist() - - #split camera cov cam_cov = est_stds[6*(numCams-1):] std_cameras = list() - + offset=0 for cidx, cam in enumerate(cself.cameras): nt = cam.geometry.minimalDimensionsDistortion() + \ cam.geometry.minimalDimensionsProjection() + \ cam.geometry.minimalDimensionsShutter() - + std_cameras.append( cam_cov[offset:offset+nt].flatten().tolist() ) offset = offset+nt - + return std_baselines, std_cameras def printParameters(cself, dest=sys.stdout): #get the covariances std_baselines, std_cameras = recoverCovariance(cself) - #print cameras + #print cameras print >> dest, "Camera-system parameters:" for cidx, cam in enumerate(cself.cameras): d = cam.geometry.projection().distortion().getParameters().flatten(1) @@ -595,10 +607,14 @@ def printParameters(cself, dest=sys.stdout): print >> dest, "\t projection: %s +- %s" % (p, np.array(dp)) #reproj error statistics - corners, reprojs, rerrs = getReprojectionErrors(cself, cidx) - if len(rerrs)>0: + corners, reprojs, rerrs = getReprojectionErrors(cself, cidx) + + if len(rerrs)>0 and rerrs.count(None)!= len(rerrs): me, se = getReprojectionErrorStatistics(rerrs) print >> dest, "\t reprojection error: [%f, %f] +- [%f, %f]" % (me[0], me[1], se[0], se[1]) + else: + print "No error data for cam{0}".format(cidx) + print >> dest #print baselines diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py index fdfd0d95f..603f06e9e 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py @@ -244,11 +244,12 @@ def getTargetPoseGuess(self, timestamp, cameras, baselines_HL=[]): #transform it back to cam0 (T_t_cN --> T_t_c0) T_cN_c0 = sm.Transformation() + for baseline_HL in baselines_HL[0:cam_id_max]: T_cN_c0 = baseline_HL * T_cN_c0 - + T_t_c0 = T_t_cN * T_cN_c0 - + return T_t_c0 #get all observations between two cameras diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus index 626349ea0..12db0d0b8 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_focus +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_focus @@ -2,7 +2,7 @@ print "importing libraries" import rospy import cv2 -import cv +#import cv import numpy as np from sensor_msgs.msg import Image from cv_bridge import CvBridge, CvBridgeError @@ -12,7 +12,7 @@ class CameraFocus: def __init__(self, topic): self.topic = topic self.windowNameOrig = "Camera: {0}".format(self.topic) - cv2.namedWindow(self.windowNameOrig, 2) + #cv2.namedWindow(self.windowNameOrig, 2) self.bridge = CvBridge() self.image_sub = rospy.Subscriber(self.topic, Image, self.callback) @@ -53,9 +53,11 @@ if __name__ == "__main__": camval = CameraFocus(topic) rospy.init_node('kalibr_validator', anonymous=True) + rate = rospy.Rate(10) # 10hz try: rospy.spin() + rate.sleep() except KeyboardInterrupt: print "Shutting down" - cv.DestroyAllWindows() + cv2.destroyAllWindows() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator index e58a739d0..723364928 100755 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_validator +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_validator @@ -6,7 +6,7 @@ from cv_bridge import CvBridge, CvBridgeError import message_filters print "importing libraries" -import cv +#import cv import cv2 import sm import aslam_cv as acv @@ -26,6 +26,7 @@ import igraph # make numpy print prettier np.set_printoptions(suppress=True) + class CalibrationTargetDetector(object): def __init__(self, camera, targetConfig): @@ -53,6 +54,15 @@ class CalibrationTargetDetector(object): targetParams['tagCols'], targetParams['tagSize'], targetParams['tagSpacing']) + elif (targetType == 'assymetric_aprilgrid'): + vectorTags =[] + for tag in targetParams['tags']: + structTag = acv_april.TargetPoint() + structTag.x = tag["pos"][0] + structTag.y = tag["pos"][1] + structTag.size =tag["size"] + vectorTags.append(structTag) + grid = acv_april.GridCalibrationTargetAssymetricAprilgrid(vectorTags) else: raise RuntimeError( "Unknown calibration target." ) @@ -102,7 +112,7 @@ class CameraChainValidator(object): edge["A"][cidx_dest] = self.prepareStereoRectificationMaps(cidx_src, cidx_dest) #register the callback for the synchronized images - sync_sub = message_filters.TimeSynchronizer([val.image_sub for val in self.monovalidators],1) + sync_sub = message_filters.ApproximateTimeSynchronizer([val.image_sub for val in self.monovalidators],2,0.02) sync_sub.registerCallback(self.synchronizedCallback) #initialize message throttler @@ -110,7 +120,7 @@ class CameraChainValidator(object): def synchronizedCallback(self, *cam_msgs): #throttle image processing - rate = 2 #Hz + rate = 10 #Hz timeNow = time.time() if (timeNow-self.timeLast < 1.0/rate) and self.timeLast!=0: return @@ -146,13 +156,13 @@ class CameraChainValidator(object): validator.undist_image = validator.undistorter.undistortImage(np_image) #generate a mono view for each cam - validator.generateMonoview(np_image, observation, success) + #validator.generateMonoview(np_image, observation, success) #generate all rectification views for edge in self.G.es: cidx_src = edge.source cidx_dest = edge.target - self.generatePairView(cidx_src, cidx_dest) + self.generatePairView(cidx_src, cidx_dest,timestamp) cv2.waitKey(1) @@ -191,11 +201,10 @@ class CameraChainValidator(object): return np_rect_image - def generatePairView(self, camAnr, camBnr): + def generatePairView(self, camAnr, camBnr,timestamp): #prepare the window windowName = "Rectified view (cam{0} and cam{1})".format(camAnr, camBnr) - cv2.namedWindow(windowName, 1) - + #cv2.namedWindow(windowName, 1) #get the mono validators for each cam camA = self.monovalidators[camAnr] camB = self.monovalidators[camBnr] @@ -257,7 +266,7 @@ class CameraChainValidator(object): #draw some epilines np_image_rect = cv2.cvtColor(np_image_rect, cv2.COLOR_GRAY2BGR) - n=10 + n=40 for i in range(0,n): y = np_image_rect.shape[0]*i/n cv2.line(np_image_rect, (0,y), (2*np_image_rect.shape[1],y),(0,255,0)); @@ -295,10 +304,10 @@ class CameraChainValidator(object): else: cv2.putText(np_image_rect, "Detection failed...", (np_image_rect.shape[0]/2,np_image_rect.shape[1]/5), cv2.FONT_HERSHEY_SIMPLEX, fontScale=2, color=(0, 0, 255), thickness=3) - + + cv2.imshow(windowName, np_image_rect) - def prepareStereoRectificationMaps(self, camAnr, camBnr): #get the camera parameters for the undistorted cameras camIdealA = self.monovalidators[camAnr].undist_camera.projection().getParameters().flatten() @@ -306,7 +315,7 @@ class CameraChainValidator(object): camIdealA = np.array([[camIdealA[0],0,camIdealA[2]], [0,camIdealA[1],camIdealA[3]], [0,0,1]]) camIdealB = np.array([[camIdealB[0],0,camIdealB[2]], [0,camIdealB[1],camIdealB[3]], [0,0,1]]) imageSize = (self.monovalidators[camAnr].undist_camera.projection().ru(), self.monovalidators[camAnr].undist_camera.projection().rv()) - + #get the baseline between the cams baseline_BA = self.getTransformationCamFromTo(camAnr, camBnr) @@ -380,8 +389,8 @@ class MonoCameraValidator(object): self.topic = camConfig.getRosTopic() self.windowName = "Camera: {0}".format(self.topic) - cv2.namedWindow(self.windowName, 1) - + #cv2.namedWindow(self.windowName, 1) + #register the cam topic to the message synchronizer self.image_sub = message_filters.Subscriber(self.topic, Image) @@ -450,7 +459,7 @@ class MonoCameraValidator(object): px_fix = int(px * 2**shift) py_fix = int(py * 2**shift) radius_fix = int(radius * 2**shift) - cv2.circle(np_image, (px_fix, py_fix), radius=radius_fix, color=(255,255,0), thickness=thickness, shift=shift, lineType=cv2.CV_AA) + cv2.circle(np_image, (px_fix, py_fix), radius=radius_fix, color=(255,255,0), thickness=thickness, shift=shift, lineType=cv2.LINE_AA) else: cv2.putText(np_image, "Detection failed...", (np_image.shape[0]/2,np_image.shape[1]/5), cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.5, color=(0, 0, 255), thickness=2) @@ -470,7 +479,7 @@ if __name__ == "__main__": sm.setLoggingLevel(sm.LoggingLevel.Debug) else: sm.setLoggingLevel(sm.LoggingLevel.Info) - + #load the configuration yamls targetConfig = kc.ConfigReader.CalibrationTargetParameters(parsed.targetYaml) @@ -483,9 +492,11 @@ if __name__ == "__main__": #ros message loops rospy.init_node('kalibr_validator', anonymous=True) + rate = rospy.Rate(10) # 10hz try: rospy.spin() + rate.sleep() except KeyboardInterrupt: print "Shutting down" - cv.DestroyAllWindows() + cv2.destroyAllWindows() diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py b/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py index a90e1e4d9..607257f25 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/ConfigReader.py @@ -9,650 +9,659 @@ import sm class AslamCamera(object): - def __init__(self, camera_model, intrinsics, dist_model, dist_coeff, resolution): - #setup the aslam camera - if camera_model == 'pinhole': - focalLength = intrinsics[0:2] - principalPoint = intrinsics[2:4] - - if dist_model == 'radtan': - dist = cv.RadialTangentialDistortion(dist_coeff[0], dist_coeff[1], - dist_coeff[2], dist_coeff[3]) - - proj = cv.DistortedPinholeProjection(focalLength[0], focalLength[1], - principalPoint[0], principalPoint[1], - resolution[0], resolution[1], - dist) - - self.geometry = cv.DistortedPinholeCameraGeometry(proj) - - self.frameType = cv.DistortedPinholeFrame - self.keypointType = cv.Keypoint2 - self.reprojectionErrorType = cvb.DistortedPinholeReprojectionErrorSimple - self.undistorterType = cv.PinholeUndistorterNoMask - - elif dist_model == 'equidistant': - dist = cv.EquidistantDistortion(dist_coeff[0], dist_coeff[1], dist_coeff[2], dist_coeff[3]) - - proj = cv.EquidistantPinholeProjection(focalLength[0], focalLength[1], - principalPoint[0], principalPoint[1], - resolution[0], resolution[1], - dist) - - self.geometry = cv.EquidistantDistortedPinholeCameraGeometry(proj) - - self.frameType = cv.EquidistantDistortedPinholeFrame - self.keypointType = cv.Keypoint2 - self.reprojectionErrorType = cvb.EquidistantDistortedPinholeReprojectionErrorSimple - self.undistorterType = cv.EquidistantPinholeUndistorterNoMask - - elif dist_model == 'fov': - dist = cv.FovDistortion(dist_coeff[0]) - - proj = cv.FovPinholeProjection(focalLength[0], focalLength[1], - principalPoint[0], principalPoint[1], - resolution[0], resolution[1], dist) - - self.geometry = cv.FovDistortedPinholeCameraGeometry(proj) - - self.frameType = cv.FovDistortedPinholeFrame - self.keypointType = cv.Keypoint2 - self.reprojectionErrorType = cvb.FovDistortedPinholeReprojectionErrorSimple - self.undistorterType = cv.FovPinholeUndistorterNoMask - else: - proj = cv.PinholeProjection(focalLength[0], focalLength[1], - principalPoint[0], principalPoint[1], - resolution[0], resolution[1]) - - self.camera = cv.PinholeCameraGeometry(proj) - - self.frameType = cv.PinholeFrame - self.keypointType = cv.Keypoint2 - self.reprojectionErrorType = cvb.PinholeReprojectionErrorSimple - - elif camera_model == 'omni': - xi_omni = intrinsics[0] - focalLength = intrinsics[1:3] - principalPoint = intrinsics[3:5] - - if dist_model == 'radtan': - dist = cv.RadialTangentialDistortion(dist_coeff[0], dist_coeff[1], - dist_coeff[2], dist_coeff[3]) - - proj = cv.DistortedOmniProjection(xi_omni, focalLength[0], focalLength[1], - principalPoint[0], principalPoint[1], - resolution[0], resolution[1], - dist) + def __init__(self, camera_model, intrinsics, dist_model, dist_coeff, resolution): + #setup the aslam camera + if camera_model == 'pinhole': + focalLength = intrinsics[0:2] + principalPoint = intrinsics[2:4] + + if dist_model == 'radtan': + dist = cv.RadialTangentialDistortion(dist_coeff[0], dist_coeff[1], + dist_coeff[2], dist_coeff[3]) + + proj = cv.DistortedPinholeProjection(focalLength[0], focalLength[1], + principalPoint[0], principalPoint[1], + resolution[0], resolution[1], + dist) + + self.geometry = cv.DistortedPinholeCameraGeometry(proj) + + self.frameType = cv.DistortedPinholeFrame + self.keypointType = cv.Keypoint2 + self.reprojectionErrorType = cvb.DistortedPinholeReprojectionErrorSimple + self.undistorterType = cv.PinholeUndistorterNoMask + + elif dist_model == 'equidistant': + dist = cv.EquidistantDistortion(dist_coeff[0], dist_coeff[1], dist_coeff[2], dist_coeff[3]) + + proj = cv.EquidistantPinholeProjection(focalLength[0], focalLength[1], + principalPoint[0], principalPoint[1], + resolution[0], resolution[1], + dist) + + self.geometry = cv.EquidistantDistortedPinholeCameraGeometry(proj) + + self.frameType = cv.EquidistantDistortedPinholeFrame + self.keypointType = cv.Keypoint2 + self.reprojectionErrorType = cvb.EquidistantDistortedPinholeReprojectionErrorSimple + self.undistorterType = cv.EquidistantPinholeUndistorterNoMask + + elif dist_model == 'fov': + dist = cv.FovDistortion(dist_coeff[0]) + + proj = cv.FovPinholeProjection(focalLength[0], focalLength[1], + principalPoint[0], principalPoint[1], + resolution[0], resolution[1], dist) + + self.geometry = cv.FovDistortedPinholeCameraGeometry(proj) + + self.frameType = cv.FovDistortedPinholeFrame + self.keypointType = cv.Keypoint2 + self.reprojectionErrorType = cvb.FovDistortedPinholeReprojectionErrorSimple + self.undistorterType = cv.FovPinholeUndistorterNoMask + else: + proj = cv.PinholeProjection(focalLength[0], focalLength[1], + principalPoint[0], principalPoint[1], + resolution[0], resolution[1]) + + self.camera = cv.PinholeCameraGeometry(proj) + + self.frameType = cv.PinholeFrame + self.keypointType = cv.Keypoint2 + self.reprojectionErrorType = cvb.PinholeReprojectionErrorSimple + + elif camera_model == 'omni': + xi_omni = intrinsics[0] + focalLength = intrinsics[1:3] + principalPoint = intrinsics[3:5] + + if dist_model == 'radtan': + dist = cv.RadialTangentialDistortion(dist_coeff[0], dist_coeff[1], + dist_coeff[2], dist_coeff[3]) + + proj = cv.DistortedOmniProjection(xi_omni, focalLength[0], focalLength[1], + principalPoint[0], principalPoint[1], + resolution[0], resolution[1], + dist) - - self.geometry = cv.DistortedOmniCameraGeometry(proj) - - self.frameType = cv.DistortedOmniFrame - self.keypointType = cv.Keypoint2 - self.reprojectionErrorType = cvb.DistortedOmniReprojectionErrorSimple - self.undistorterType = cv.OmniUndistorterNoMask - - elif dist_model == 'equidistant': - - print "Omni with equidistant model not yet supported!" - sys.exit(0) - - dist = cv.EquidistantPinholeProjection(dist_coeff[0], dist_coeff[1], - dist_coeff[2], dist_coeff[3]) - - proj = cv.EquidistantOmniProjection(xi_omni, focalLength[0], focalLength[1], - principalPoint[0], principalPoint[1], - resolution[0], resolution[1], - dist) - - - self.geometry = cv.EquidistantDistortedOmniCameraGeometry(proj) - - self.frameType = cv.DistortedOmniFrame - self.keypointType = cv.Keypoint2 - self.reprojectionErrorType = cvb.EquidistantDistortedOmniReprojectionErrorSimple + + self.geometry = cv.DistortedOmniCameraGeometry(proj) + + self.frameType = cv.DistortedOmniFrame + self.keypointType = cv.Keypoint2 + self.reprojectionErrorType = cvb.DistortedOmniReprojectionErrorSimple + self.undistorterType = cv.OmniUndistorterNoMask + + elif dist_model == 'equidistant': + + print "Omni with equidistant model not yet supported!" + sys.exit(0) + + dist = cv.EquidistantPinholeProjection(dist_coeff[0], dist_coeff[1], + dist_coeff[2], dist_coeff[3]) + + proj = cv.EquidistantOmniProjection(xi_omni, focalLength[0], focalLength[1], + principalPoint[0], principalPoint[1], + resolution[0], resolution[1], + dist) + + + self.geometry = cv.EquidistantDistortedOmniCameraGeometry(proj) + + self.frameType = cv.DistortedOmniFrame + self.keypointType = cv.Keypoint2 + self.reprojectionErrorType = cvb.EquidistantDistortedOmniReprojectionErrorSimple - elif dist_model == 'none': - self.raiseError("camera model omni needs a distortion model! (none is invalid)") - - else: - self.raiseError("Unknown camera model") - - @classmethod - def fromParameters(cls, params): - #get the data - camera_model, intrinsics = params.getIntrinsics() - dist_model, dist_coeff = params.getDistortion() - resolution = params.getResolution() - return AslamCamera(camera_model, intrinsics, dist_model, dist_coeff, resolution) - + elif dist_model == 'none': + self.raiseError("camera model omni needs a distortion model! (none is invalid)") + + else: + self.raiseError("Unknown camera model") + + @classmethod + def fromParameters(cls, params): + #get the data + camera_model, intrinsics = params.getIntrinsics() + dist_model, dist_coeff = params.getDistortion() + resolution = params.getResolution() + return AslamCamera(camera_model, intrinsics, dist_model, dist_coeff, resolution) + #wrapper to ctach all KeyError exception (field missing in yaml ...) def catch_keyerror(f): - @functools.wraps(f) - def func(*args, **kwargs): - try: - return f(*args, **kwargs) - except KeyError as e: - args[0].raiseError("Field {0} missing in file: {1}".format(e, args[0].yamlFile)) - return func + @functools.wraps(f) + def func(*args, **kwargs): + try: + return f(*args, **kwargs) + except KeyError as e: + args[0].raiseError("Field {0} missing in file: {1}".format(e, args[0].yamlFile)) + return func class ParametersBase(object): - def __init__(self, yamlFile, name, createYaml=False): - #load the tree - self.yamlFile = yamlFile - self.name = name - - #load yaml if we don't create a new one - if not createYaml: - self.data = self.readYaml() - else: - self.data = dict() + def __init__(self, yamlFile, name, createYaml=False): + #load the tree + self.yamlFile = yamlFile + self.name = name + + #load yaml if we don't create a new one + if not createYaml: + self.data = self.readYaml() + else: + self.data = dict() - #load from yaml file - def readYaml(self): - try: - with open(self.yamlFile, 'r') as f: - data = yaml.safe_load(f) - f.close() - except: - self.raiseError( "Could not read configuration from {0}".format(self.yamlFile) ) - - return data - - #write to yaml file - def writeYaml(self, filename=None): - if not filename: - filename = self.yamlFile #overwrite source file - - try: - with open(filename, 'w') as outfile: - outfile.write( yaml.dump(self.data) ) - except: - self.raiseError( "Could not write configuration to {0}".format(self.yamlFile) ) + #load from yaml file + def readYaml(self): + try: + with open(self.yamlFile, 'r') as f: + data = yaml.safe_load(f) + f.close() + except: + self.raiseError( "Could not read configuration from {0}".format(self.yamlFile) ) + + return data + + #write to yaml file + def writeYaml(self, filename=None): + if not filename: + filename = self.yamlFile #overwrite source file + + try: + with open(filename, 'w') as outfile: + outfile.write( yaml.dump(self.data) ) + except: + self.raiseError( "Could not write configuration to {0}".format(self.yamlFile) ) - def getYamlDict(self): - return self.data + def getYamlDict(self): + return self.data - def setYamlDict(self, yamldict): - self.data = yamldict - - def raiseError(self, message): - header = "[{0} Reader]: ".format(self.name) - raise RuntimeError( "{0}{1}".format(header, message) ) + def setYamlDict(self, yamldict): + self.data = yamldict + + def raiseError(self, message): + header = "[{0} Reader]: ".format(self.name) + raise RuntimeError( "{0}{1}".format(header, message) ) class CameraParameters(ParametersBase): - def __init__(self, yamlFile, createYaml=False): - ParametersBase.__init__(self, yamlFile, "CameraConfig", createYaml) + def __init__(self, yamlFile, createYaml=False): + ParametersBase.__init__(self, yamlFile, "CameraConfig", createYaml) - ################################################### - # Accessors - ################################################### - #rostopic - def checkRosTopic(self, topic): - if not isinstance(topic, str): - self.raiseError("rostopic has to be a string") - - @catch_keyerror - def getRosTopic(self): - self.checkRosTopic(self.data["rostopic"]) - return self.data["rostopic"] - - def setRosTopic(self, topic): - self.checkRosTopic(topic) - self.data["rostopic"] = topic - - #intrinsics - def checkIntrinsics(self, model, intrinsics): - cameraModels = ['pinhole', - 'omni'] - - if model not in cameraModels: - self.raiseError('Unknown camera model; available models: {0}. )'.format(cameraModels) ) - - if model == 'pinhole': - if len(intrinsics) != 4: - self.raiseError("invalid intrinsics for pinhole; [fu, fv, pu, pv]") + ################################################### + # Accessors + ################################################### + #rostopic + def checkRosTopic(self, topic): + if not isinstance(topic, str): + self.raiseError("rostopic has to be a string") + + @catch_keyerror + def getRosTopic(self): + self.checkRosTopic(self.data["rostopic"]) + return self.data["rostopic"] + + def setRosTopic(self, topic): + self.checkRosTopic(topic) + self.data["rostopic"] = topic + + #intrinsics + def checkIntrinsics(self, model, intrinsics): + cameraModels = ['pinhole', + 'omni'] + + if model not in cameraModels: + self.raiseError('Unknown camera model; available models: {0}. )'.format(cameraModels) ) + + if model == 'pinhole': + if len(intrinsics) != 4: + self.raiseError("invalid intrinsics for pinhole; [fu, fv, pu, pv]") - focalLength = intrinsics[0:2] - principalPoint = intrinsics[2:4] - - elif model == 'omni': - if len(intrinsics) != 5: - self.raiseError("invalid intrinsics for omni; [xi, fu, fv, pu, pv]") - - xi_omni = intrinsics[0] - focalLength = intrinsics[1:3] - principalPoint = intrinsics[3:5] - - if xi_omni<0: - self.raiseError("invalid xi_omni (xi>0)" ) - - if not isinstance(focalLength[0],float) or not isinstance(focalLength[1],float) or focalLength[0] < 0.0 or focalLength[1] < 0.0: - self.raiseError("invalid focalLength (2 floats)") - - if principalPoint[0] < 0.0 or principalPoint[1] < 0.0 or not isinstance(principalPoint[0],float) or not isinstance(principalPoint[1],float): - self.raiseError("invalid principalPoint") - - @catch_keyerror - def getIntrinsics(self): - self.checkIntrinsics(self.data["camera_model"], - self.data["intrinsics"]) - - return self.data["camera_model"], self.data["intrinsics"] - - def setIntrinsics(self, model, intrinsics): - self.checkIntrinsics(model, intrinsics) - - self.data["camera_model"] = model - self.data["intrinsics"] = [ float(val) for val in intrinsics ] - - #distortion - def checkDistortion(self, model, coeffs): - distortionModelsAndNumParams = {'radtan': 4, - 'equidistant': 4, - 'fov': 1, - 'none': 0} - - if model not in distortionModelsAndNumParams: - self.raiseError('Unknown distortion model. Supported models: {0}. )'.format(distortionModels) ) - - if len(coeffs) != distortionModelsAndNumParams[model]: - self.raiseError("distortion model requires 4 coefficients") - - @catch_keyerror - def getDistortion(self): - self.checkDistortion(self.data["distortion_model"], - self.data["distortion_coeffs"]) - return self.data["distortion_model"], self.data["distortion_coeffs"] - - def setDistortion(self, model, coeffs): - self.checkDistortion(model, coeffs) - self.data["distortion_model"] = model - self.data["distortion_coeffs"] = [ float(val) for val in coeffs ] + focalLength = intrinsics[0:2] + principalPoint = intrinsics[2:4] + + elif model == 'omni': + if len(intrinsics) != 5: + self.raiseError("invalid intrinsics for omni; [xi, fu, fv, pu, pv]") + + xi_omni = intrinsics[0] + focalLength = intrinsics[1:3] + principalPoint = intrinsics[3:5] + + if xi_omni<0: + self.raiseError("invalid xi_omni (xi>0)" ) + + if not isinstance(focalLength[0],float) or not isinstance(focalLength[1],float) or focalLength[0] < 0.0 or focalLength[1] < 0.0: + self.raiseError("invalid focalLength (2 floats)") + + if principalPoint[0] < 0.0 or principalPoint[1] < 0.0 or not isinstance(principalPoint[0],float) or not isinstance(principalPoint[1],float): + self.raiseError("invalid principalPoint") + + @catch_keyerror + def getIntrinsics(self): + self.checkIntrinsics(self.data["camera_model"], + self.data["intrinsics"]) + + return self.data["camera_model"], self.data["intrinsics"] + + def setIntrinsics(self, model, intrinsics): + self.checkIntrinsics(model, intrinsics) + + self.data["camera_model"] = model + self.data["intrinsics"] = [ float(val) for val in intrinsics ] + + #distortion + def checkDistortion(self, model, coeffs): + distortionModelsAndNumParams = {'radtan': 4, + 'equidistant': 4, + 'fov': 1, + 'none': 0} + + if model not in distortionModelsAndNumParams: + self.raiseError('Unknown distortion model. Supported models: {0}. )'.format(distortionModels) ) + + if len(coeffs) != distortionModelsAndNumParams[model]: + self.raiseError("distortion model requires 4 coefficients") + + @catch_keyerror + def getDistortion(self): + self.checkDistortion(self.data["distortion_model"], + self.data["distortion_coeffs"]) + return self.data["distortion_model"], self.data["distortion_coeffs"] + + def setDistortion(self, model, coeffs): + self.checkDistortion(model, coeffs) + self.data["distortion_model"] = model + self.data["distortion_coeffs"] = [ float(val) for val in coeffs ] - #resolution - def checkResolution(self, resolution): - if len(resolution)!=2 or not isinstance(resolution[0],int) or not isinstance(resolution[1],int): - self.raiseError("invalid resolution") - - @catch_keyerror - def getResolution(self): - self.checkResolution(self.data["resolution"]) - return self.data["resolution"] - - def setResolution(self, resolution): - self.checkResolution(resolution) - self.data["resolution"] = resolution - - ################################################### - # Helpers - ################################################### - def printDetails(self, dest=sys.stdout): - #get the data - camera_model, intrinsics = self.getIntrinsics() - dist_model, dist_coeff = self.getDistortion() - resolution = self.getResolution() - - if camera_model == 'pinhole': - focalLength = intrinsics[0:2] - principalPoint = intrinsics[2:4] - - elif camera_model == 'omni': - xi_omni = intrinsics[0] - focalLength = intrinsics[1:3] - principalPoint = intrinsics[3:5] - - print >> dest, " Camera model: {0}".format(camera_model) - print >> dest, " Focal length: {0}".format(focalLength) - print >> dest, " Principal point: {0}".format(principalPoint) - if camera_model == 'omni': - print >> dest, " Omni xi: {0}".format(xi_omni) - print >> dest, " Distortion model: {0}".format(dist_model) - print >> dest, " Distortion coefficients: {0}".format(dist_coeff) + #resolution + def checkResolution(self, resolution): + if len(resolution)!=2 or not isinstance(resolution[0],int) or not isinstance(resolution[1],int): + self.raiseError("invalid resolution") + + @catch_keyerror + def getResolution(self): + self.checkResolution(self.data["resolution"]) + return self.data["resolution"] + + def setResolution(self, resolution): + self.checkResolution(resolution) + self.data["resolution"] = resolution + + ################################################### + # Helpers + ################################################### + def printDetails(self, dest=sys.stdout): + #get the data + camera_model, intrinsics = self.getIntrinsics() + dist_model, dist_coeff = self.getDistortion() + resolution = self.getResolution() + + if camera_model == 'pinhole': + focalLength = intrinsics[0:2] + principalPoint = intrinsics[2:4] + + elif camera_model == 'omni': + xi_omni = intrinsics[0] + focalLength = intrinsics[1:3] + principalPoint = intrinsics[3:5] + + print >> dest, " Camera model: {0}".format(camera_model) + print >> dest, " Focal length: {0}".format(focalLength) + print >> dest, " Principal point: {0}".format(principalPoint) + if camera_model == 'omni': + print >> dest, " Omni xi: {0}".format(xi_omni) + print >> dest, " Distortion model: {0}".format(dist_model) + print >> dest, " Distortion coefficients: {0}".format(dist_coeff) class ImuParameters(ParametersBase): - def __init__(self, yamlFile, createYaml=False): - ParametersBase.__init__(self, yamlFile, "ImuConfig", createYaml) + def __init__(self, yamlFile, createYaml=False): + ParametersBase.__init__(self, yamlFile, "ImuConfig", createYaml) - ################################################### - # Accessors - ################################################### - #rostopic - def checkRosTopic(self, topic): - if not isinstance(topic, str): - self.raiseError("rostopic has to be a string") - - @catch_keyerror - def getRosTopic(self): - self.checkRosTopic(self.data["rostopic"]) - return self.data["rostopic"] - - def setRosTopic(self, topic): - self.checkRosTopic(topic) - self.data["rostopic"] = topic - - #update rate - def checkUpdateRate(self, update_rate): - if update_rate <= 0.0: - self.raiseError("invalid update_rate") - - @catch_keyerror - def getUpdateRate(self): - self.checkUpdateRate(self.data["update_rate"]) - return self.data["update_rate"] - - def setUpdateRate(self, update_rate): - self.checkUpdateRate(update_rate) - self.data["update_rate"] = update_rate - - #accelerometer statistics - def checkAccelerometerStatistics(self, noise_density, random_walk): - if noise_density <= 0.0: - self.raiseError("invalid accelerometer_noise_density") - if random_walk <= 0.0: - self.raiseError("invalid accelerometer_random_walk") - - @catch_keyerror - def getAccelerometerStatistics(self): - self.checkAccelerometerStatistics(self.data["accelerometer_noise_density"], self.data["accelerometer_random_walk"]) - accelUncertaintyDiscrete = self.data["accelerometer_noise_density"] / math.sqrt(1.0/self.getUpdateRate()) - return accelUncertaintyDiscrete, self.data["accelerometer_random_walk"], self.data["accelerometer_noise_density"] - - def setAccelerometerStatistics(self, noise_density, random_walk): - self.checkAccelerometerStatistics(noise_density, random_walk) - self.data["accelerometer_noise_density"] = accelerometer_noise_density - self.data["accelerometer_random_walk"] = accelerometer_random_walk - - #gyro statistics - def checkGyroStatistics(self, noise_density, random_walk): - if noise_density <= 0.0: - self.raiseError("invalid gyroscope_noise_density") - if random_walk <= 0.0: - self.raiseError("invalid gyroscope_random_walk") - - @catch_keyerror - def getGyroStatistics(self): - self.checkGyroStatistics(self.data["gyroscope_noise_density"], self.data["gyroscope_random_walk"]) - gyroUncertaintyDiscrete = self.data["gyroscope_noise_density"] / math.sqrt(1.0/self.getUpdateRate()) - return gyroUncertaintyDiscrete, self.data["gyroscope_random_walk"], self.data["gyroscope_noise_density"] - - def setGyroStatistics(self, noise_density, random_walk): - self.checkGyroStatistics(noise_density, random_walk) - self.data["gyroscope_noise_density"] = accelerometer_noise_density - self.data["gyroscope_random_walk"] = accelerometer_random_walk + ################################################### + # Accessors + ################################################### + #rostopic + def checkRosTopic(self, topic): + if not isinstance(topic, str): + self.raiseError("rostopic has to be a string") + + @catch_keyerror + def getRosTopic(self): + self.checkRosTopic(self.data["rostopic"]) + return self.data["rostopic"] + + def setRosTopic(self, topic): + self.checkRosTopic(topic) + self.data["rostopic"] = topic + + #update rate + def checkUpdateRate(self, update_rate): + if update_rate <= 0.0: + self.raiseError("invalid update_rate") + + @catch_keyerror + def getUpdateRate(self): + self.checkUpdateRate(self.data["update_rate"]) + return self.data["update_rate"] + + def setUpdateRate(self, update_rate): + self.checkUpdateRate(update_rate) + self.data["update_rate"] = update_rate + + #accelerometer statistics + def checkAccelerometerStatistics(self, noise_density, random_walk): + if noise_density <= 0.0: + self.raiseError("invalid accelerometer_noise_density") + if random_walk <= 0.0: + self.raiseError("invalid accelerometer_random_walk") + + @catch_keyerror + def getAccelerometerStatistics(self): + self.checkAccelerometerStatistics(self.data["accelerometer_noise_density"], self.data["accelerometer_random_walk"]) + accelUncertaintyDiscrete = self.data["accelerometer_noise_density"] / math.sqrt(1.0/self.getUpdateRate()) + return accelUncertaintyDiscrete, self.data["accelerometer_random_walk"], self.data["accelerometer_noise_density"] + + def setAccelerometerStatistics(self, noise_density, random_walk): + self.checkAccelerometerStatistics(noise_density, random_walk) + self.data["accelerometer_noise_density"] = accelerometer_noise_density + self.data["accelerometer_random_walk"] = accelerometer_random_walk + + #gyro statistics + def checkGyroStatistics(self, noise_density, random_walk): + if noise_density <= 0.0: + self.raiseError("invalid gyroscope_noise_density") + if random_walk <= 0.0: + self.raiseError("invalid gyroscope_random_walk") + + @catch_keyerror + def getGyroStatistics(self): + self.checkGyroStatistics(self.data["gyroscope_noise_density"], self.data["gyroscope_random_walk"]) + gyroUncertaintyDiscrete = self.data["gyroscope_noise_density"] / math.sqrt(1.0/self.getUpdateRate()) + return gyroUncertaintyDiscrete, self.data["gyroscope_random_walk"], self.data["gyroscope_noise_density"] + + def setGyroStatistics(self, noise_density, random_walk): + self.checkGyroStatistics(noise_density, random_walk) + self.data["gyroscope_noise_density"] = accelerometer_noise_density + self.data["gyroscope_random_walk"] = accelerometer_random_walk - - ################################################### - # Helpers - ################################################### - def printDetails(self, dest=sys.stdout): - #get the data - update_rate = self.getUpdateRate() - accelUncertaintyDiscrete, accelRandomWalk, accelUncertainty = self.getAccelerometerStatistics() - gyroUncertaintyDiscrete, gyroRandomWalk, gyroUncertainty = self.getGyroStatistics() + + ################################################### + # Helpers + ################################################### + def printDetails(self, dest=sys.stdout): + #get the data + update_rate = self.getUpdateRate() + accelUncertaintyDiscrete, accelRandomWalk, accelUncertainty = self.getAccelerometerStatistics() + gyroUncertaintyDiscrete, gyroRandomWalk, gyroUncertainty = self.getGyroStatistics() - print >> dest, " Update rate: {0}".format(update_rate) - print >> dest, " Accelerometer:" - print >> dest, " Noise density: {0} ".format(accelUncertainty) - print >> dest, " Noise density (discrete): {0} ".format(accelUncertaintyDiscrete) - print >> dest, " Random walk: {0}".format(accelRandomWalk) - print >> dest, " Gyroscope:" - print >> dest, " Noise density: {0}".format(gyroUncertainty) - print >> dest, " Noise density (discrete): {0} ".format(gyroUncertaintyDiscrete) - print >> dest, " Random walk: {0}".format(gyroRandomWalk) - + print >> dest, " Update rate: {0}".format(update_rate) + print >> dest, " Accelerometer:" + print >> dest, " Noise density: {0} ".format(accelUncertainty) + print >> dest, " Noise density (discrete): {0} ".format(accelUncertaintyDiscrete) + print >> dest, " Random walk: {0}".format(accelRandomWalk) + print >> dest, " Gyroscope:" + print >> dest, " Noise density: {0}".format(gyroUncertainty) + print >> dest, " Noise density (discrete): {0} ".format(gyroUncertaintyDiscrete) + print >> dest, " Random walk: {0}".format(gyroRandomWalk) + class ImuSetParameters(ParametersBase): - def __init__(self, yamlFile, createYaml=False): - ParametersBase.__init__(self, yamlFile, "ImuSetConfig", createYaml) - self.imuCount = 0 - - def addImuParameters(self, imu_parameters, name=None): - if name is None: - name = "imu%d" % self.imuCount - self.imuCount += 1 - self.data[name] = imu_parameters.getYamlDict() + def __init__(self, yamlFile, createYaml=False): + ParametersBase.__init__(self, yamlFile, "ImuSetConfig", createYaml) + self.imuCount = 0 + + def addImuParameters(self, imu_parameters, name=None): + if name is None: + name = "imu%d" % self.imuCount + self.imuCount += 1 + self.data[name] = imu_parameters.getYamlDict() class CalibrationTargetParameters(ParametersBase): - def __init__(self, yamlFile, createYaml=False): - ParametersBase.__init__(self, yamlFile, "CalibrationTargetConfig", createYaml) - - ################################################### - # Accessors - ################################################### - def checkTargetType(self, target_type): - targetTypes = ['aprilgrid', - 'checkerboard', - 'circlegrid'] - - if target_type not in targetTypes: - self.raiseError('Unknown calibration target type. Supported types: {0}. )'.format(targetTypes) ) - - @catch_keyerror - def getTargetType(self): - self.checkTargetType(self.data["target_type"]) - return self.data["target_type"] - - @catch_keyerror - def getTargetParams(self): - #read target specidic data - targetType = self.getTargetType() - - if targetType == 'checkerboard': - try: - targetRows = self.data["targetRows"] - targetCols = self.data["targetCols"] - rowSpacingMeters = self.data["rowSpacingMeters"] - colSpacingMeters = self.data["colSpacingMeters"] - except KeyError, e: - self.raiseError("Calibration target configuration in {0} is missing the field: {1}".format(self.yamlFile, str(e)) ) - - if not isinstance(targetRows,int) or targetRows < 3: - errList.append("invalid targetRows (int>=3)") - if not isinstance(targetCols,int) or targetCols < 3: - errList.append("invalid targetCols (int>=3)") - if not isinstance(rowSpacingMeters,float) or rowSpacingMeters <= 0.0: - errList.append("invalid rowSpacingMeters (float)") - if not isinstance(colSpacingMeters,float) or colSpacingMeters <= 0.0: - errList.append("invalid colSpacingMeters (float)") - - targetParams = {'targetRows': targetRows, - 'targetCols': targetCols, - 'rowSpacingMeters': rowSpacingMeters, - 'colSpacingMeters': colSpacingMeters, - 'targetType': targetType} - - elif targetType == 'circlegrid': - try: - targetRows = self.data["targetRows"] - targetCols = self.data["targetCols"] - spacingMeters = self.data["spacingMeters"] - asymmetricGrid = self.data["asymmetricGrid"] - except KeyError, e: - self.raiseError("Calibration target configuration in {0} is missing the field: {1}".format(self.yamlFile, str(e)) ) - - if not isinstance(targetRows,int) or targetRows < 3: - errList.append("invalid targetRows (int>=3)") - if not isinstance(targetCols,int) or targetCols < 3: - errList.append("invalid targetCols (int>=3)") - if not isinstance(spacingMeters,float) or spacingMeters <= 0.0: - errList.append("invalid spacingMeters (float)") - if not isinstance(asymmetricGrid,bool): - errList.append("invalid asymmetricGrid (bool)") - - targetParams = {'targetRows': targetRows, - 'targetCols': targetCols, - 'spacingMeters': spacingMeters, - 'asymmetricGrid': asymmetricGrid, - 'targetType': targetType} - - elif targetType == 'aprilgrid': - try: - tagRows = self.data["tagRows"] - tagCols = self.data["tagCols"] - tagSize = self.data["tagSize"] - tagSpacing = self.data["tagSpacing"] - except KeyError, e: - self.raiseError("Calibration target configuration in {0} is missing the field: {1}".format(self.yamlFile, str(e)) ) - - if not isinstance(tagRows,int) or tagRows < 3: - errList.append("invalid tagRows (int>=3)") - if not isinstance(tagCols,int) or tagCols < 3: - errList.append("invalid tagCols (int>=3)") - if not isinstance(tagSize,float) or tagSize <= 0.0: - errList.append("invalid tagSize (float)") - if not isinstance(tagSpacing,float) or tagSpacing <= 0.0: - errList.append("invalid tagSpacing (float)") - - targetParams = {'tagRows': tagRows, - 'tagCols': tagCols, - 'tagSize': tagSize, - 'tagSpacing': tagSpacing, - 'targetType': targetType} - - return targetParams - - ################################################### - # Helpers - ################################################### - def printDetails(self, dest=sys.stdout): - - targetType = self.getTargetType() - targetParams = self.getTargetParams() - - print >> dest, " Type: {0}".format(targetType) - - if targetType == 'checkerboard': - print >> dest, " Rows" - print >> dest, " Count: {0}".format(targetParams['targetRows']) - print >> dest, " Distance: {0} [m]".format(targetParams['rowSpacingMeters']) - print >> dest, " Cols" - print >> dest, " Count: {0}".format(targetParams['targetCols']) - print >> dest, " Distance: {0} [m]".format(targetParams['colSpacingMeters']) - elif targetType == 'aprilgrid': - print >> dest, " Tags: " - print >> dest, " Rows: {0}".format(targetParams['tagRows']) - print >> dest, " Cols: {0}".format(targetParams['tagCols']) - print >> dest, " Size: {0} [m]".format(targetParams['tagSize']) - print >> dest, " Spacing {0} [m]".format( targetParams['tagSize']*targetParams['tagSpacing'] ) + def __init__(self, yamlFile, createYaml=False): + ParametersBase.__init__(self, yamlFile, "CalibrationTargetConfig", createYaml) + + ################################################### + # Accessors + ################################################### + def checkTargetType(self, target_type): + targetTypes = ['aprilgrid', + 'checkerboard', + 'circlegrid', + 'assymetric_aprilgrid'] + + if target_type not in targetTypes: + self.raiseError('Unknown calibration target type. Supported types: {0}. )'.format(targetTypes) ) + + @catch_keyerror + def getTargetType(self): + self.checkTargetType(self.data["target_type"]) + return self.data["target_type"] + + @catch_keyerror + def getTargetParams(self): + #read target specidic data + targetType = self.getTargetType() + + if targetType == 'checkerboard': + try: + targetRows = self.data["targetRows"] + targetCols = self.data["targetCols"] + rowSpacingMeters = self.data["rowSpacingMeters"] + colSpacingMeters = self.data["colSpacingMeters"] + except KeyError, e: + self.raiseError("Calibration target configuration in {0} is missing the field: {1}".format(self.yamlFile, str(e)) ) + + if not isinstance(targetRows,int) or targetRows < 3: + errList.append("invalid targetRows (int>=3)") + if not isinstance(targetCols,int) or targetCols < 3: + errList.append("invalid targetCols (int>=3)") + if not isinstance(rowSpacingMeters,float) or rowSpacingMeters <= 0.0: + errList.append("invalid rowSpacingMeters (float)") + if not isinstance(colSpacingMeters,float) or colSpacingMeters <= 0.0: + errList.append("invalid colSpacingMeters (float)") + + targetParams = {'targetRows': targetRows, + 'targetCols': targetCols, + 'rowSpacingMeters': rowSpacingMeters, + 'colSpacingMeters': colSpacingMeters, + 'targetType': targetType} + + elif targetType == 'circlegrid': + try: + targetRows = self.data["targetRows"] + targetCols = self.data["targetCols"] + spacingMeters = self.data["spacingMeters"] + asymmetricGrid = self.data["asymmetricGrid"] + except KeyError, e: + self.raiseError("Calibration target configuration in {0} is missing the field: {1}".format(self.yamlFile, str(e)) ) + + if not isinstance(targetRows,int) or targetRows < 3: + errList.append("invalid targetRows (int>=3)") + if not isinstance(targetCols,int) or targetCols < 3: + errList.append("invalid targetCols (int>=3)") + if not isinstance(spacingMeters,float) or spacingMeters <= 0.0: + errList.append("invalid spacingMeters (float)") + if not isinstance(asymmetricGrid,bool): + errList.append("invalid asymmetricGrid (bool)") + + targetParams = {'targetRows': targetRows, + 'targetCols': targetCols, + 'spacingMeters': spacingMeters, + 'asymmetricGrid': asymmetricGrid, + 'targetType': targetType} + + elif targetType == 'aprilgrid': + try: + tagRows = self.data["tagRows"] + tagCols = self.data["tagCols"] + tagSize = self.data["tagSize"] + tagSpacing = self.data["tagSpacing"] + except KeyError, e: + self.raiseError("Calibration target configuration in {0} is missing the field: {1}".format(self.yamlFile, str(e)) ) + + if not isinstance(tagRows,int) or tagRows < 3: + errList.append("invalid tagRows (int>=3)") + if not isinstance(tagCols,int) or tagCols < 3: + errList.append("invalid tagCols (int>=3)") + if not isinstance(tagSize,float) or tagSize <= 0.0: + errList.append("invalid tagSize (float)") + if not isinstance(tagSpacing,float) or tagSpacing <= 0.0: + errList.append("invalid tagSpacing (float)") + + targetParams = {'tagRows': tagRows, + 'tagCols': tagCols, + 'tagSize': tagSize, + 'tagSpacing': tagSpacing, + 'targetType': targetType} + elif targetType == 'assymetric_aprilgrid': + try: + tags = self.data["tags"] + except KeyError, e: + self.raiseError("Calibration target configuration in {0} is missing the field: {1}".format(self.yamlFile, str(e)) ) + + targetParams = {'tags': tags, + 'targetType': targetType} + return targetParams + + ################################################### + # Helpers + ################################################### + def printDetails(self, dest=sys.stdout): + + targetType = self.getTargetType() + targetParams = self.getTargetParams() + + print >> dest, " Type: {0}".format(targetType) + + if targetType == 'checkerboard': + print >> dest, " Rows" + print >> dest, " Count: {0}".format(targetParams['targetRows']) + print >> dest, " Distance: {0} [m]".format(targetParams['rowSpacingMeters']) + print >> dest, " Cols" + print >> dest, " Count: {0}".format(targetParams['targetCols']) + print >> dest, " Distance: {0} [m]".format(targetParams['colSpacingMeters']) + elif targetType == 'aprilgrid': + print >> dest, " Tags: " + print >> dest, " Rows: {0}".format(targetParams['tagRows']) + print >> dest, " Cols: {0}".format(targetParams['tagCols']) + print >> dest, " Size: {0} [m]".format(targetParams['tagSize']) + print >> dest, " Spacing {0} [m]".format( targetParams['tagSize']*targetParams['tagSpacing'] ) + elif targetType == 'aprilgrid_assymetric': + print >> dest, " Number of Tags: {0}".format(len(targetParams['tags'])) - class CameraChainParameters(ParametersBase): - def __init__(self, yamlFile, createYaml=False): - ParametersBase.__init__(self, yamlFile, "CameraChainParameters", createYaml) - - ################################################### - # Accessors - ################################################### - def addCameraAtEnd(self, cam): - if not isinstance(cam, CameraParameters): - raise RuntimeError("addCamera() requires a CameraParameters object") - - camNr = len(self.data) - self.data["cam{0}".format(camNr)] = cam.getYamlDict() - - @catch_keyerror - def getCameraParameters(self, nr): - if nr >= self.numCameras(): - self.raiseError("out-of-range: camera index not in camera chain!") + def __init__(self, yamlFile, createYaml=False): + ParametersBase.__init__(self, yamlFile, "CameraChainParameters", createYaml) + + ################################################### + # Accessors + ################################################### + def addCameraAtEnd(self, cam): + if not isinstance(cam, CameraParameters): + raise RuntimeError("addCamera() requires a CameraParameters object") + + camNr = len(self.data) + self.data["cam{0}".format(camNr)] = cam.getYamlDict() + + @catch_keyerror + def getCameraParameters(self, nr): + if nr >= self.numCameras(): + self.raiseError("out-of-range: camera index not in camera chain!") - param = CameraParameters("TEMP_CONFIG", createYaml=True) - param.setYamlDict( self.data['cam{0}'.format(nr)] ) - return param - - def setExtrinsicsLastCamToHere(self, camNr, extrinsics): - if camNr==0: - raise RuntimeError("setExtrinsicsLastCamToHere(): can't set extrinsics for first cam in chain (cam0=base)") - if not isinstance(extrinsics, sm.Transformation): - raise RuntimeError("setExtrinsicsLastCamToHere(): provide extrinsics as an sm.Transformation object") - - self.data["cam{0}".format(camNr)]['T_cn_cnm1'] = extrinsics.T().tolist() + param = CameraParameters("TEMP_CONFIG", createYaml=True) + param.setYamlDict( self.data['cam{0}'.format(nr)] ) + return param + + def setExtrinsicsLastCamToHere(self, camNr, extrinsics): + if camNr==0: + raise RuntimeError("setExtrinsicsLastCamToHere(): can't set extrinsics for first cam in chain (cam0=base)") + if not isinstance(extrinsics, sm.Transformation): + raise RuntimeError("setExtrinsicsLastCamToHere(): provide extrinsics as an sm.Transformation object") + + self.data["cam{0}".format(camNr)]['T_cn_cnm1'] = extrinsics.T().tolist() - @catch_keyerror - def getExtrinsicsLastCamToHere(self, camNr): - if camNr==0: - raise RuntimeError("setExtrinsicsLastCamToHere(): can't get extrinsics for first camera in chain (cam0=base)") - - if camNr >= self.numCameras(): - self.raiseError("out-of-range: baseline index not in camera chain!") - - try: - trafo = sm.Transformation( np.array(self.data["cam{0}".format(camNr)]['T_cn_cnm1']) ) - t = trafo.T() #for error checking - except: - self.raiseError("invalid camera baseline (cam{0} in {1})".format(camNr, self.yamlFile)) - return trafo - - def setExtrinsicsImuToCam(self, camNr, extrinsics): - if not isinstance(extrinsics, sm.Transformation): - raise RuntimeError("setExtrinsicsImuToCam(): provide extrinsics as an sm.Transformation object") - - self.data["cam{0}".format(camNr)]['T_cam_imu'] = extrinsics.T().tolist() - - @catch_keyerror - def getExtrinsicsImuToCam(self, camNr): - if camNr >= self.numCameras(): - self.raiseError("out-of-range: T_imu_cam not in chain!") - - try: - trafo = sm.Transformation( np.array(self.data["cam{0}".format(camNr)]['T_cam_imu']) ) - t = trafo.T() #for error checking - except: - self.raiseError("invalid T_cam_imu (cam{0} in {1})".format(camNr, self.yamlFile)) - return trafo - - def checkTimeshiftCamImu(self, camNr, time_shift): - if camNr >= self.numCameras(): - self.raiseError("out-of-range: imu-cam trafo not in chain!") - - if not isinstance(time_shift, float): - self.raiseError("invalid timeshift cam-imu (cam{0} in {1})".format(camNr, self.yamlFile)) - - @catch_keyerror - def getTimeshiftCamImu(self, camNr): - self.checkTimeshiftCamImu(camNr, self.data["cam{0}".format(camNr)]["timeshift_cam_imu"]) - return self.data["cam{0}".format(camNr)]["timeshift_cam_imu"] - - def setTimeshiftCamImu(self, camNr, time_shift): - self.checkTimeshiftCamImu(camNr, time_shift) - self.data["cam{0}".format(camNr)]["timeshift_cam_imu"] = time_shift + @catch_keyerror + def getExtrinsicsLastCamToHere(self, camNr): + if camNr==0: + raise RuntimeError("setExtrinsicsLastCamToHere(): can't get extrinsics for first camera in chain (cam0=base)") + + if camNr >= self.numCameras(): + self.raiseError("out-of-range: baseline index not in camera chain!") + + try: + trafo = sm.Transformation( np.array(self.data["cam{0}".format(camNr)]['T_cn_cnm1']) ) + t = trafo.T() #for error checking + except: + self.raiseError("invalid camera baseline (cam{0} in {1})".format(camNr, self.yamlFile)) + return trafo + + def setExtrinsicsImuToCam(self, camNr, extrinsics): + if not isinstance(extrinsics, sm.Transformation): + raise RuntimeError("setExtrinsicsImuToCam(): provide extrinsics as an sm.Transformation object") + + self.data["cam{0}".format(camNr)]['T_cam_imu'] = extrinsics.T().tolist() + + @catch_keyerror + def getExtrinsicsImuToCam(self, camNr): + if camNr >= self.numCameras(): + self.raiseError("out-of-range: T_imu_cam not in chain!") + + try: + trafo = sm.Transformation( np.array(self.data["cam{0}".format(camNr)]['T_cam_imu']) ) + t = trafo.T() #for error checking + except: + self.raiseError("invalid T_cam_imu (cam{0} in {1})".format(camNr, self.yamlFile)) + return trafo + + def checkTimeshiftCamImu(self, camNr, time_shift): + if camNr >= self.numCameras(): + self.raiseError("out-of-range: imu-cam trafo not in chain!") + + if not isinstance(time_shift, float): + self.raiseError("invalid timeshift cam-imu (cam{0} in {1})".format(camNr, self.yamlFile)) + + @catch_keyerror + def getTimeshiftCamImu(self, camNr): + self.checkTimeshiftCamImu(camNr, self.data["cam{0}".format(camNr)]["timeshift_cam_imu"]) + return self.data["cam{0}".format(camNr)]["timeshift_cam_imu"] + + def setTimeshiftCamImu(self, camNr, time_shift): + self.checkTimeshiftCamImu(camNr, time_shift) + self.data["cam{0}".format(camNr)]["timeshift_cam_imu"] = time_shift - def checkCamOverlaps(self, camNr, overlap_list): - if camNr >= self.numCameras(): - self.raiseError("out-of-range: camera id of {0}".format(camNr)) - - for cam_id in overlap_list: - if cam_id >= self.numCameras(): - self.raiseError("out-of-range: camera id of {0}".format(cam_id)) + def checkCamOverlaps(self, camNr, overlap_list): + if camNr >= self.numCameras(): + self.raiseError("out-of-range: camera id of {0}".format(camNr)) + + for cam_id in overlap_list: + if cam_id >= self.numCameras(): + self.raiseError("out-of-range: camera id of {0}".format(cam_id)) - @catch_keyerror - def getCamOverlaps(self, camNr): - self.checkCamOverlaps(camNr, self.data["cam{0}".format(camNr)]["cam_overlaps"]) - return self.data["cam{0}".format(camNr)]["cam_overlaps"] - - def setCamOverlaps(self, camNr, overlap_list): - self.checkCamOverlaps(camNr, overlap_list) - self.data["cam{0}".format(camNr)]["cam_overlaps"] = overlap_list - - def numCameras(self): - return len(self.data) - - def printDetails(self, dest=sys.stdout): - for camNr in range(0, self.numCameras()): - print "Camera chain - cam{0}:".format(camNr) - camConfig = self.getCameraParameters(camNr) - camConfig.printDetails(dest) - - #print baseline if available - try: - T = self.getExtrinsicsLastCamToHere(camNr) - print >> dest, " baseline:", T.T() - except: - print >> dest, " baseline: no data available" - pass + @catch_keyerror + def getCamOverlaps(self, camNr): + self.checkCamOverlaps(camNr, self.data["cam{0}".format(camNr)]["cam_overlaps"]) + return self.data["cam{0}".format(camNr)]["cam_overlaps"] + + def setCamOverlaps(self, camNr, overlap_list): + self.checkCamOverlaps(camNr, overlap_list) + self.data["cam{0}".format(camNr)]["cam_overlaps"] = overlap_list + + def numCameras(self): + return len(self.data) + + def printDetails(self, dest=sys.stdout): + for camNr in range(0, self.numCameras()): + print "Camera chain - cam{0}:".format(camNr) + camConfig = self.getCameraParameters(camNr) + camConfig.printDetails(dest) + + #print baseline if available + try: + T = self.getExtrinsicsLastCamToHere(camNr) + print >> dest, " baseline:", T.T() + except: + print >> dest, " baseline: no data available" + pass diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py b/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py index 76cf29da0..a5b173ec5 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py @@ -50,7 +50,7 @@ def __init__(self, bagfile, imagetopic, bag_from_to=None, perform_synchronizatio # sort the indices by header.stamp self.indices = self.sortByTime(self.indices) - + # go through the bag and remove the indices outside the timespan [bag_start_time, bag_end_time] if bag_from_to: self.indices = self.truncateIndicesFromTime(self.indices, bag_from_to) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py b/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py index 2b7ca395d..4db1bea3d 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_common/TargetExtractor.py @@ -17,12 +17,10 @@ def multicoreExtractionWrapper(detector, taskq, resultq, clearImages, noTransfor idx = task[0] stamp = task[1] image = task[2] - if noTransformation: success, obs = detector.findTargetNoTransformation(stamp, np.array(image)) else: success, obs = detector.findTarget(stamp, np.array(image)) - if clearImages: obs.clearImage() if success: @@ -32,11 +30,10 @@ def extractCornersFromDataset(dataset, detector, multithreading=False, numProces print "Extracting calibration target corners" targetObservations = [] numImages = dataset.numImages() - # prepare progess bar iProgress = sm.Progress2(numImages) iProgress.sample() - + if multithreading: if not numProcesses: numProcesses = max(1,multiprocessing.cpu_count()-1) @@ -44,18 +41,17 @@ def extractCornersFromDataset(dataset, detector, multithreading=False, numProces manager = multiprocessing.Manager() resultq = manager.Queue() manager2 = multiprocessing.Manager() - taskq = manager2.Queue() - + taskq = manager2.Queue() for idx, (timestamp, image) in enumerate(dataset.readDataset()): taskq.put( (idx, timestamp, image) ) - + plist=list() + for pidx in range(0, numProcesses): detector_copy = copy.copy(detector) - p = multiprocessing.Process(target=multicoreExtractionWrapper, args=(detector_copy, taskq, resultq, clearImages, noTransformation, )) + p = multiprocessing.Process(target=multicoreExtractionWrapper, args=(detector_copy, taskq, resultq, clearImages, noTransformation)) p.start() plist.append(p) - #wait for results last_done=0 while 1: @@ -71,7 +67,7 @@ def extractCornersFromDataset(dataset, detector, multithreading=False, numProces resultq.put('STOP') except Exception, e: raise RuntimeError("Exception during multithreaded extraction: {0}".format(e)) - + #get result sorted by time (=idx) if resultq.qsize() > 1: targetObservations = [[]]*(resultq.qsize()-1) @@ -82,6 +78,7 @@ def extractCornersFromDataset(dataset, detector, multithreading=False, numProces else: targetObservations=[] + #single threaded implementation else: for timestamp, image in dataset.readDataset(): diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py index c81d77fff..537aac9c6 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py @@ -102,6 +102,23 @@ def setupCalibrationTarget(self, targetConfig, showExtraction=False, showReproj= targetParams['tagSize'], targetParams['tagSpacing'], options) + elif targetType == 'assymetric_aprilgrid': + options = acv_april.AprilgridOptions() + #enforce more than one row --> pnp solution can be bad if all points are almost on a line... + options.minTagsForValidObs = 1 + options.showExtractionVideo = showExtraction + #options.maxSubpixDisplacement2 = 2 + options.doSubpixRefinement = False + vectorTags =[] + for tag in targetParams['tags']: + structTag = acv_april.TargetPoint() + structTag.x = tag["pos"][0] + structTag.y = tag["pos"][1] + structTag.size =tag["size"] + vectorTags.append(structTag) + + grid = acv_april.GridCalibrationTargetAssymetricAprilgrid(vectorTags, + options) else: raise RuntimeError( "Unknown calibration target." )