Skip to content

Commit

Permalink
Simplify and fixes for opencv5
Browse files Browse the repository at this point in the history
  • Loading branch information
jlblancoc committed Feb 16, 2024
1 parent 031d1f0 commit acd3f9f
Show file tree
Hide file tree
Showing 26 changed files with 59 additions and 3,028 deletions.
23 changes: 11 additions & 12 deletions apps/benchmarking-image-features/src/visual_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -240,16 +240,16 @@ VisualOdometry::VisualOdometry()
* Generate VO main funciton *
************************************************************************************************/
Mat VisualOdometry::generateVO(
CFeatureExtraction fext, int numFeats,
CFeatureExtraction fExt, int NumFeats,
std::array<std::string, 3> file_paths, int feat_type)
{
string dataset = file_paths[0];
string groundtruth = file_paths[1];
string calibration_file = file_paths[2];
cnt.setValue(0);
// this->detector_selected = detector_selected;
this->fext = fext;
this->numFeats = numFeats;
this->fext = fExt;
this->numFeats = NumFeats;
Mat img_1, img_2;
Mat R_f, t_f; // the final rotation and tranlation vectors containing the
// transform
Expand Down Expand Up @@ -318,9 +318,9 @@ Mat VisualOdometry::generateVO(
// double focal = 718.8560;
// cv::Point2d pp(607.1928, 185.2157);
// recovering the pose and the essential matrix
Mat E, R, t, mask;
E = findEssentialMat(points2, points1, focal, pp, RANSAC, 0.999, 1.0, mask);
recoverPose(E, points2, points1, R, t, focal, pp, mask);
Mat E, R, t;
E = findEssentialMat(points2, points1, focal, pp);
cv::recoverPose(E, points2, points1, R, t, focal, pp);

Mat prevImage = img_2;
Mat currImage;
Expand Down Expand Up @@ -354,13 +354,12 @@ Mat VisualOdometry::generateVO(
img3.loadFromFile(filename);

cvtColor(currImage_c, currImage, COLOR_BGR2GRAY);
vector<uchar> status;
vector<uchar> Status;
featureTracking(
prevImage, currImage, prevFeatures, currFeatures, status);
prevImage, currImage, prevFeatures, currFeatures, Status);

E = findEssentialMat(
currFeatures, prevFeatures, focal, pp, RANSAC, 0.999, 1.0, mask);
recoverPose(E, currFeatures, prevFeatures, R, t, focal, pp, mask);
E = findEssentialMat(currFeatures, prevFeatures, focal, pp, RANSAC);
recoverPose(E, currFeatures, prevFeatures, R, t, focal, pp);

Mat prevPts(2, prevFeatures.size(), CV_64F),
currPts(2, currFeatures.size(), CV_64F);
Expand Down Expand Up @@ -394,7 +393,7 @@ Mat VisualOdometry::generateVO(
{
featureDetection(img3, prevFeatures, feat_type);
featureTracking(
prevImage, currImage, prevFeatures, currFeatures, status);
prevImage, currImage, prevFeatures, currFeatures, Status);
}

prevImage = currImage.clone();
Expand Down
12 changes: 1 addition & 11 deletions apps/camera-calib/CDlgCalibWizardOnline.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -135,13 +135,6 @@ CDlgCalibWizardOnline::CDlgCalibWizardOnline(
StaticBoxSizer4->Add(FlexGridSizer17, 1, wxEXPAND, 0);
FlexGridSizer6->Add(
StaticBoxSizer4, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 2);
wxString __wxRadioBoxChoices_1[2] = {
_("OpenCV\'s default"), _("Scaramuzza et al.\'s")};
rbMethod = new wxRadioBox(
this, ID_RADIOBOX1, _(" Detector method: "), wxDefaultPosition,
wxDefaultSize, 2, __wxRadioBoxChoices_1, 1, 0, wxDefaultValidator,
_T("ID_RADIOBOX1"));
FlexGridSizer6->Add(rbMethod, 1, wxEXPAND, 2);
StaticBoxSizer5 =
new wxStaticBoxSizer(wxHORIZONTAL, this, _(" Size of quads (in mm): "));
FlexGridSizer18 = new wxFlexGridSizer(1, 4, 0, 0);
Expand Down Expand Up @@ -352,8 +345,6 @@ void CDlgCalibWizardOnline::OntimCaptureTrigger(wxTimerEvent& event)
m_check_size_x = this->edSizeX->GetValue();
m_check_size_y = this->edSizeY->GetValue();
m_normalize_image = this->cbNormalize->GetValue();
m_useScaramuzzaAlternativeDetector =
this->rbMethod->GetSelection() == 1;

CObservation::Ptr obs = m_video->getNextFrame();
ASSERT_(obs);
Expand Down Expand Up @@ -524,8 +515,7 @@ void CDlgCalibWizardOnline::threadProcessCorners()
bool foundCorners = mrpt::vision::findChessboardCorners(
obj->m_threadImgToProcess->image, obj->m_threadResults,
obj->m_check_size_x, obj->m_check_size_y,
obj->m_normalize_image,
obj->m_useScaramuzzaAlternativeDetector);
obj->m_normalize_image);

// cout << "corners: " << obj->m_threadResults.size() <<
// endl;
Expand Down
2 changes: 0 additions & 2 deletions apps/camera-calib/CDlgCalibWizardOnline.h
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,6 @@ class CDlgCalibWizardOnline : public wxDialog
wxTextCtrl* edLengthY;
wxButton* btnClose;
wxCheckBox* cbNormalize;
wxRadioBox* rbMethod;
mrpt::gui::wxMRPTImageControl* m_realtimeview;
wxSpinCtrl* edSizeY;
wxStaticText* StaticText1;
Expand Down Expand Up @@ -118,7 +117,6 @@ class CDlgCalibWizardOnline : public wxDialog
unsigned int m_check_size_x;
unsigned int m_check_size_y;
bool m_normalize_image;
bool m_useScaramuzzaAlternativeDetector;

mrpt::hwdrivers::CCameraSensor::Ptr m_video;

Expand Down
12 changes: 1 addition & 11 deletions apps/camera-calib/CDlgPoseEst.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -146,13 +146,6 @@ CDlgPoseEst::CDlgPoseEst(
StaticBoxSizer4->Add(FlexGridSizer17, 1, wxEXPAND, 0);
FlexGridSizer6->Add(
StaticBoxSizer4, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 2);
wxString __wxRadioBoxChoices_1[2] = {
_("OpenCV\'s default"), _("Scaramuzza et al.\'s")};
rbMethod = new wxRadioBox(
this, ID_RADIOBOX1, _(" Detector method: "), wxDefaultPosition,
wxDefaultSize, 2, __wxRadioBoxChoices_1, 1, 0, wxDefaultValidator,
_T("ID_RADIOBOX1"));
FlexGridSizer6->Add(rbMethod, 1, wxEXPAND, 2);
StaticBoxSizer5 =
new wxStaticBoxSizer(wxHORIZONTAL, this, _(" Size of quads (in mm): "));
FlexGridSizer18 = new wxFlexGridSizer(1, 4, 0, 0);
Expand Down Expand Up @@ -411,8 +404,6 @@ void CDlgPoseEst::OntimCaptureTrigger(wxTimerEvent& event)
m_check_size_y = this->edSizeY->GetValue();

m_normalize_image = this->cbNormalize->GetValue();
m_useScaramuzzaAlternativeDetector =
this->rbMethod->GetSelection() == 1;

CObservation::Ptr obs = m_video->getNextFrame();
ASSERT_(obs);
Expand Down Expand Up @@ -531,8 +522,7 @@ void CDlgPoseEst::threadProcessCorners()
bool foundCorners = mrpt::vision::findChessboardCorners(
obj->m_threadImgToProcess->image, obj->m_threadResults,
obj->m_check_size_x, obj->m_check_size_y,
obj->m_normalize_image,
obj->m_useScaramuzzaAlternativeDetector);
obj->m_normalize_image);

if (!foundCorners) obj->m_threadResults.clear();
else
Expand Down
2 changes: 0 additions & 2 deletions apps/camera-calib/CDlgPoseEst.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,6 @@ class CDlgPoseEst : public wxDialog
wxTextCtrl* edLengthY;
wxButton* btnClose;
wxCheckBox* cbNormalize;
wxRadioBox* rbMethod;
mrpt::gui::wxMRPTImageControl* m_realtimeview;
wxSpinCtrl* edSizeY;
wxStaticText* StaticText1;
Expand Down Expand Up @@ -132,7 +131,6 @@ class CDlgPoseEst : public wxDialog
unsigned int m_check_size_x;
unsigned int m_check_size_y;
bool m_normalize_image;
bool m_useScaramuzzaAlternativeDetector;
mrpt::hwdrivers::CCameraSensor::Ptr m_video;
CMyGLCanvas* m_3Dview_cam;
mrpt::vision::pnp::CPnP pnp_algos;
Expand Down
12 changes: 1 addition & 11 deletions apps/camera-calib/camera_calib_guiMain.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -268,13 +268,6 @@ camera_calib_guiDialog::camera_calib_guiDialog(wxWindow* parent, wxWindowID id)
StaticBoxSizer4->Add(FlexGridSizer17, 1, wxEXPAND, 0);
FlexGridSizer6->Add(
StaticBoxSizer4, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 2);
wxString __wxRadioBoxChoices_1[2] = {
_("OpenCV\'s default"), _("Scaramuzza et al.\'s")};
rbMethod = new wxRadioBox(
this, ID_RADIOBOX1, _(" Detector method: "), wxDefaultPosition,
wxDefaultSize, 2, __wxRadioBoxChoices_1, 0, 0, wxDefaultValidator,
_T("ID_RADIOBOX1"));
FlexGridSizer6->Add(rbMethod, 1, wxEXPAND, 2);
StaticBoxSizer5 =
new wxStaticBoxSizer(wxHORIZONTAL, this, _(" Size of quads (in mm): "));
FlexGridSizer18 = new wxFlexGridSizer(1, 4, 0, 0);
Expand Down Expand Up @@ -604,16 +597,13 @@ void camera_calib_guiDialog::OnbtnRunCalibClick(wxCommandEvent& event)

const bool normalize_image = cbNormalize->GetValue();

const bool useScaramuzzaAlternativeDetector =
rbMethod->GetSelection() == 1;

wxBusyCursor waitcur;

bool res = mrpt::vision::checkerBoardCameraCalibration(
lst_images, check_size_x, check_size_y,
check_squares_length_X_meters, check_squares_length_Y_meters,
camera_params, normalize_image, nullptr /* MSE */,
false /* skip draw */, useScaramuzzaAlternativeDetector);
false /* skip draw */);

refreshDisplayedImage();

Expand Down
1 change: 0 additions & 1 deletion apps/camera-calib/camera_calib_guiMain.h
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,6 @@ class camera_calib_guiDialog : public wxDialog
wxTextCtrl* edLengthX;
CMyGLCanvas* m_3Dview;
wxNotebook* Notebook1;
wxRadioBox* rbMethod;
wxButton* btnSave;
wxButton* btnAbout;
wxStaticText* StaticText2;
Expand Down
3 changes: 3 additions & 0 deletions doc/source/doxygen-docs/changelog.md
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,11 @@
- Changes in libraries:
- \ref mrpt_maps_grp:
- mrpt::maps::CHeightGridMap2D: now supports integrating any point-cloud observation.
- \ref mrpt_vision_grp:
- Remove functions that were problematic with opencv 5: mrpt::vision::findMultipleChessboardsCorners()
- Others:
- Fix Debian appstream warnings on mrpt-apps.
- Fix build against opencv 5.

# Version 2.11.9: Released Feb 11th, 2024
- Changes in libraries:
Expand Down

This file was deleted.

1 change: 0 additions & 1 deletion doc/source/examples.rst
Original file line number Diff line number Diff line change
Expand Up @@ -140,6 +140,5 @@ Python examples are `here <python_examples.html>`_.
page_vision_create_video_file_example.rst
page_vision_feature_extraction.rst
page_vision_keypoint_matching_example.rst
page_vision_multiple_checkerboards.rst
page_vision_stereo_calib_example.rst
page_vision_stereo_rectify.rst
12 changes: 5 additions & 7 deletions libs/hwdrivers/include/mrpt/hwdrivers/CImageGrabber_OpenCV.h
Original file line number Diff line number Diff line change
Expand Up @@ -20,11 +20,11 @@ namespace mrpt::hwdrivers
enum TCameraType
{
CAMERA_CV_AUTODETECT = 0,
CAMERA_CV_DC1394,
CAMERA_CV_VFL,
CAMERA_CV_VFW,
CAMERA_CV_MIL,
CAMERA_CV_DSHOW
CAMERA_CV_DC1394 = 1,
CAMERA_CV_VFL = 2,
// CAMERA_CV_VFW = 3, // removed feb-2024
// CAMERA_CV_MIL = 4, // removed feb-2024
CAMERA_CV_DSHOW = 5
};

/** Options used when creating an OpenCV capture object
Expand Down Expand Up @@ -109,7 +109,5 @@ MRPT_ENUM_TYPE_BEGIN(mrpt::hwdrivers::TCameraType)
MRPT_FILL_ENUM_MEMBER(mrpt::hwdrivers, CAMERA_CV_AUTODETECT);
MRPT_FILL_ENUM_MEMBER(mrpt::hwdrivers, CAMERA_CV_DC1394);
MRPT_FILL_ENUM_MEMBER(mrpt::hwdrivers, CAMERA_CV_VFL);
MRPT_FILL_ENUM_MEMBER(mrpt::hwdrivers, CAMERA_CV_VFW);
MRPT_FILL_ENUM_MEMBER(mrpt::hwdrivers, CAMERA_CV_MIL);
MRPT_FILL_ENUM_MEMBER(mrpt::hwdrivers, CAMERA_CV_DSHOW);
MRPT_ENUM_TYPE_END()
22 changes: 10 additions & 12 deletions libs/hwdrivers/src/CImageGrabber_OpenCV.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,12 +45,10 @@ CImageGrabber_OpenCV::CImageGrabber_OpenCV(
int cv_cap_indx = 0;
switch (cameraType)
{
case CAMERA_CV_AUTODETECT: cv_cap_indx = CV_CAP_ANY; break;
case CAMERA_CV_DC1394: cv_cap_indx = CV_CAP_DC1394; break;
case CAMERA_CV_VFL: cv_cap_indx = CV_CAP_V4L; break;
case CAMERA_CV_VFW: cv_cap_indx = CV_CAP_VFW; break;
case CAMERA_CV_MIL: cv_cap_indx = CV_CAP_MIL; break;
case CAMERA_CV_DSHOW: cv_cap_indx = CV_CAP_DSHOW; break;
case CAMERA_CV_AUTODETECT: cv_cap_indx = cv::CAP_ANY; break;
case CAMERA_CV_DC1394: cv_cap_indx = cv::CAP_DC1394; break;
case CAMERA_CV_VFL: cv_cap_indx = cv::CAP_V4L; break;
case CAMERA_CV_DSHOW: cv_cap_indx = cv::CAP_DSHOW; break;
default: THROW_EXCEPTION_FMT("Invalid camera type: %i", cameraType);
}

Expand All @@ -71,7 +69,7 @@ CImageGrabber_OpenCV::CImageGrabber_OpenCV(
// Global settings
if (options.gain != 0)
{
if (!m_capture->cap.set(CV_CAP_PROP_GAIN, options.gain))
if (!m_capture->cap.set(cv::CAP_PROP_GAIN, options.gain))
cerr << "[CImageGrabber_OpenCV] Warning: Could not set the "
"capturing gain property!"
<< endl;
Expand Down Expand Up @@ -109,7 +107,7 @@ CImageGrabber_OpenCV::CImageGrabber_OpenCV(

if (cvMode1394 > 0)
{
if (!m_capture->cap.set(CV_CAP_PROP_MODE, cvMode1394))
if (!m_capture->cap.set(cv::CAP_PROP_MODE, cvMode1394))
cerr << "[CImageGrabber_OpenCV] Warning: Could not set the "
"capturing mode "
<< cvMode1394 << " property!" << endl;
Expand All @@ -126,23 +124,23 @@ CImageGrabber_OpenCV::CImageGrabber_OpenCV(
// cerr << "[CImageGrabber_OpenCV] Warning: Could not set the RGB
// conversion property!" << endl;

if (!m_capture->cap.set(CV_CAP_PROP_FPS, options.ieee1394_fps))
if (!m_capture->cap.set(cv::CAP_PROP_FPS, options.ieee1394_fps))
cerr << "[CImageGrabber_OpenCV] Warning: Could not set the fps "
"property!"
<< endl;
}

// Settings only for V4L
if (cameraType == CAMERA_CV_AUTODETECT || cameraType == CAMERA_CV_VFL ||
cameraType == CAMERA_CV_VFW || cameraType == CAMERA_CV_DSHOW)
cameraType == CAMERA_CV_DSHOW)
{
if (options.frame_width != 0 && options.frame_height != 0)
{
// First set width then height. The first command always returns a
// error!
m_capture->cap.set(CV_CAP_PROP_FRAME_WIDTH, options.frame_width);
m_capture->cap.set(cv::CAP_PROP_FRAME_WIDTH, options.frame_width);
if (!m_capture->cap.set(
CV_CAP_PROP_FRAME_HEIGHT, options.frame_height))
cv::CAP_PROP_FRAME_HEIGHT, options.frame_height))
cerr << "[CImageGrabber_OpenCV] Warning: Could not set the "
"frame width & height property!"
<< endl;
Expand Down
8 changes: 2 additions & 6 deletions libs/vision/include/mrpt/vision/chessboard_camera_calib.h
Original file line number Diff line number Diff line change
Expand Up @@ -78,8 +78,6 @@ using TCalibrationImageList = std::map<std::string, TImageCalibData>;
* reprojection will be stored here (in pixel units).
* \param skipDrawDetectedImgs [IN] Whether to skip the generation of the
* undistorted and detected images in each TImageCalibData
* \param useScaramuzzaAlternativeDetector [IN] Whether to use an alternative
* detector. See CImage::findChessboardCorners for more deatails and references.
* \sa The <a href="http://www.mrpt.org/Application:camera-calib-gui"
* >camera-calib-gui application</a> is a user-friendly GUI to this class.
* \return false on any error (more info will be dumped to cout), or true on
Expand All @@ -91,8 +89,7 @@ bool checkerBoardCameraCalibration(
unsigned int check_size_y, double check_squares_length_X_meters,
double check_squares_length_Y_meters, mrpt::img::TCamera& out_camera_params,
bool normalize_image = true, double* out_MSE = nullptr,
bool skipDrawDetectedImgs = false,
bool useScaramuzzaAlternativeDetector = false);
bool skipDrawDetectedImgs = false);

/** \overload with matrix of intrinsic params instead of mrpt::img::TCamera
*/
Expand All @@ -102,8 +99,7 @@ bool checkerBoardCameraCalibration(
double check_squares_length_Y_meters,
mrpt::math::CMatrixDouble33& intrinsicParams,
std::vector<double>& distortionParams, bool normalize_image = true,
double* out_MSE = nullptr, bool skipDrawDetectedImgs = false,
bool useScaramuzzaAlternativeDetector = false);
double* out_MSE = nullptr, bool skipDrawDetectedImgs = false);

/** @} */ // end of grouping
} // namespace mrpt::vision
45 changes: 2 additions & 43 deletions libs/vision/include/mrpt/vision/chessboard_find_corners.h
Original file line number Diff line number Diff line change
Expand Up @@ -47,57 +47,16 @@ namespace mrpt::vision
* \param check_size_x [IN] The number of squares, in the X direction
* \param check_size_y [IN] The number of squares, in the Y direction
* \param normalize_image [IN] Whether to normalize the image before detection
* \param useScaramuzzaMethod [IN] Whether to use the alternative, more robust
*method by M. Rufli, D. Scaramuzza, and R. Siegwart.
*
* \return true on success
*
* \sa findMultipleChessboardsCorners,
*mrpt::vision::checkerBoardCameraCalibration, drawChessboardCorners
* \sa mrpt::vision::checkerBoardCameraCalibration, drawChessboardCorners
*/
bool findChessboardCorners(
const mrpt::img::CImage& img,
std::vector<mrpt::img::TPixelCoordf>& cornerCoords,
unsigned int check_size_x, unsigned int check_size_y,
bool normalize_image = true, bool useScaramuzzaMethod = false);

/** Look for the corners of one or more chessboard/checkerboards in the image.
* This method uses an improved version of OpenCV's cvFindChessboardCorners
*published
* by M. Rufli, D. Scaramuzza, and R. Siegwart. See:
*http://robotics.ethz.ch/~scaramuzza/Davide_Scaramuzza_files/Research/OcamCalib_Tutorial.htm
* and the papers:
* - 1. Scaramuzza, D., Martinelli, A. and Siegwart, R. (2006), A Toolbox
*for
*Easily Calibrating Omnidirectional Cameras, Proceedings of the IEEE/RSJ
*International Conference on Intelligent Robots and Systems (IROS 2006),
*Beijing, China, October 2006.
* - 2. Scaramuzza, D., Martinelli, A. and Siegwart, R., (2006). "A
*Flexible
*Technique for Accurate Omnidirectional Camera Calibration and Structure from
*Motion", Proceedings of IEEE International Conference of Vision Systems
*(ICVS'06), New York, January 5-7, 2006.
* - 3. Rufli, M., Scaramuzza, D., and Siegwart, R. (2008), Automatic
*Detection of Checkerboards on Blurred and Distorted Images, Proceedings of
*the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS
*2008), Nice, France, September 2008.
*
* That method has been extended in this MRPT implementation to automatically
*detect a
* number of different checkerboards in the same image.
*
* \param cornerCoords [OUT] A vector of N vectors of pixel coordinates, for
*each of the N chessboards detected.
* \param check_size_x [IN] The number of squares, in the X direction
* \param check_size_y [IN] The number of squares, in the Y direction
*
*
* \sa mrpt::vision::checkerBoardCameraCalibration, drawChessboardCorners
*/
void findMultipleChessboardsCorners(
const mrpt::img::CImage& img,
std::vector<std::vector<mrpt::img::TPixelCoordf>>& cornerCoords,
unsigned int check_size_x, unsigned int check_size_y);
bool normalize_image = true);

/** @} */
} // namespace mrpt::vision
Loading

0 comments on commit acd3f9f

Please sign in to comment.