CameraStereoTara.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
34 #include <rtabmap/utilite/UTimer.h>
37 #if CV_MAJOR_VERSION > 3
38 #include <opencv2/videoio/videoio_c.h>
39 #endif
40 
41 namespace rtabmap
42 {
43 
44 
46 {
47  return true;
48 }
49 
50 
52  int device,
53  bool rectifyImages,
54  float imageRate,
55  const Transform & localTransform) :
56  Camera(imageRate, localTransform),
57  rectifyImages_(rectifyImages),
58  usbDevice_(device)
59 {
60 
61  if ( CV_MAJOR_VERSION < 3 || ( CV_MAJOR_VERSION > 3 && CV_MINOR_VERSION < 1 ))
62  {
63  UERROR(" OpenCV %d.%d detected, Minimum OpenCV 3.2 Required ", CV_MAJOR_VERSION,CV_MINOR_VERSION);
64  exit(0);
65  }
66 }
67 
69 {
70  capture_.release();
71 }
72 
73 bool CameraStereoTara::init(const std::string & calibrationFolder, const std::string & cameraName)
74 {
75  cameraName_ = cameraName;
76  if(capture_.isOpened())
77  {
78  capture_.release();
79  }
80 
81 
82  capture_.open(usbDevice_);
83 
84  capture_.set(CV_CAP_PROP_FOURCC, CV_FOURCC('Y', '1', '6', ' '));
85  capture_.set(CV_CAP_PROP_FPS, 60);
86  capture_.set(CV_CAP_PROP_FRAME_WIDTH, 752);
87  capture_.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
88  capture_.set(CV_CAP_PROP_CONVERT_RGB,false);
89 
90  ULOGGER_DEBUG("CameraStereoTara: Usb device initialization on device %d", usbDevice_);
91 
92 
93  if (cameraName_.empty())
94  {
95  unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
96  if (guid != 0 && guid != 0xffffffff)
97  {
98  cameraName_ = uFormat("%08x", guid);
99  }
100  }
101 
102  // look for calibration files
103  if(!calibrationFolder.empty() && !cameraName_.empty())
104  {
105  if(!stereoModel_.load(calibrationFolder, cameraName_, false))
106  {
107  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
108  cameraName_.c_str(), calibrationFolder.c_str());
109  }
110  else
111  {
112  UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
113  stereoModel_.left().fx(),
114  stereoModel_.left().cx(),
115  stereoModel_.left().cy(),
117  }
118  }
119 
122  {
123  UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
124  return false;
125  }
126  return true;
127 }
128 
130 {
132 }
133 
134 std::string CameraStereoTara::getSerial() const
135 {
136  return cameraName_;
137 }
138 
140 {
142 
143  if(capture_.isOpened() )
144  {
145  cv::Mat img, leftImage, interLeavedFrame, rightImage;
146  std::vector<cv::Mat> StereoFrames;
147  interLeavedFrame.create(480, 752, CV_8UC2);
148 
149  if(!capture_.read(img))
150  {
151  return data;
152  }
153 
154  interLeavedFrame.data = img.data;
155  split(interLeavedFrame, StereoFrames);
156  leftImage=StereoFrames[0].clone();
157  rightImage=StereoFrames[1].clone();
158 
159  // Rectification
161  {
162  leftImage = stereoModel_.left().rectifyImage(leftImage);
163  rightImage = stereoModel_.right().rectifyImage(rightImage);
164  }
165 
166  if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
167  {
168  stereoModel_.setImageSize(leftImage.size());
169  }
170 
171  data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), UTimer::now());
172  }
173  else
174  {
175  ULOGGER_WARN("The camera must be initialized before requesting an image.");
176  }
177 
178  return data;
179 }
180 
181 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
int
int
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:120
UINFO
#define UINFO(...)
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::CameraStereoTara::cameraName_
std::string cameraName_
Definition: CameraStereoTara.h:69
rtabmap::StereoCameraModel::load
bool load(const std::string &directory, const std::string &cameraName, bool ignoreStereoTransform=true)
Definition: StereoCameraModel.cpp:223
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
rtabmap::StereoCameraModel::setImageSize
void setImageSize(const cv::Size &size)
Definition: StereoCameraModel.h:95
rtabmap_netvlad.img
img
Definition: rtabmap_netvlad.py:78
rtabmap::CameraStereoTara::available
static bool available()
Definition: CameraStereoTara.cpp:45
rtabmap::StereoCameraModel::baseline
double baseline() const
Definition: StereoCameraModel.h:104
rtabmap::SensorCapture::getLocalTransform
const Transform & getLocalTransform() const
Definition: SensorCapture.h:62
rtabmap::CameraStereoTara::rectifyImages_
bool rectifyImages_
Definition: CameraStereoTara.h:67
UTimer.h
rtabmap::StereoCameraModel::setLocalTransform
void setLocalTransform(const Transform &transform)
Definition: StereoCameraModel.h:118
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
rtabmap::CameraStereoTara::usbDevice_
int usbDevice_
Definition: CameraStereoTara.h:70
rtabmap::CameraStereoTara::getSerial
virtual std::string getSerial() const
Definition: CameraStereoTara.cpp:134
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::CameraStereoTara::stereoModel_
StereoCameraModel stereoModel_
Definition: CameraStereoTara.h:68
rtabmap::StereoCameraModel::right
const CameraModel & right() const
Definition: StereoCameraModel.h:123
data
int data[]
rtabmap::CameraStereoTara::captureImage
virtual SensorData captureImage(SensorCaptureInfo *info=0)
Definition: CameraStereoTara.cpp:139
UConversion.h
Some conversion functions.
rtabmap_superglue.device
string device
Definition: rtabmap_superglue.py:21
rtabmap::CameraStereoTara::~CameraStereoTara
virtual ~CameraStereoTara()
Definition: CameraStereoTara.cpp:68
rtabmap::CameraStereoTara::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraStereoTara.cpp:73
rtabmap::CameraModel::rectifyImage
cv::Mat rectifyImage(const cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Definition: CameraModel.cpp:695
rtabmap::CameraStereoTara::capture_
cv::VideoCapture capture_
Definition: CameraStereoTara.h:66
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:122
rtabmap::StereoCameraModel::isValidForProjection
bool isValidForProjection() const
Definition: StereoCameraModel.h:85
rtabmap::Camera
Definition: Camera.h:43
CameraStereoTara.h
rtabmap::CameraModel::isValidForRectification
bool isValidForRectification() const
Definition: CameraModel.h:89
UWARN
#define UWARN(...)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::StereoCameraModel::isValidForRectification
bool isValidForRectification() const
Definition: StereoCameraModel.h:86
rtabmap::Transform
Definition: Transform.h:41
rtabmap::CameraStereoTara::CameraStereoTara
CameraStereoTara(int device, bool rectifyImages=false, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraStereoTara.cpp:51
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:121
split
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
rtabmap::CameraStereoTara::isCalibrated
virtual bool isCalibrated() const
Definition: CameraStereoTara.cpp:129
UThreadC.h
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
ULOGGER_WARN
#define ULOGGER_WARN(...)
Definition: ULogger.h:55
rtabmap::SensorCapture::getNextSeqID
int getNextSeqID()
Definition: SensorCapture.h:83


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:07