mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-11 04:19:50 +08:00
Refactored Camera classes and Preferences->Source menu
This commit is contained in:
matlabbe
+23
-376
@@ -44,14 +44,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
Camera::Camera(float imageRate,
|
||||
unsigned int imageWidth,
|
||||
unsigned int imageHeight) :
|
||||
Camera::Camera(float imageRate, const Transform & localTransform) :
|
||||
_imageRate(imageRate),
|
||||
_imageWidth(imageWidth),
|
||||
_imageHeight(imageHeight),
|
||||
_mirroring(false),
|
||||
_frameRateTimer(new UTimer())
|
||||
_localTransform(localTransform),
|
||||
_targetImageSize(0,0),
|
||||
_frameRateTimer(new UTimer()),
|
||||
_seq(0)
|
||||
{
|
||||
}
|
||||
|
||||
@@ -63,397 +61,46 @@ Camera::~Camera()
|
||||
}
|
||||
}
|
||||
|
||||
void Camera::setImageSize(unsigned int width, unsigned int height)
|
||||
SensorData Camera::takeImage()
|
||||
{
|
||||
_imageWidth = width;
|
||||
_imageHeight = height;
|
||||
}
|
||||
|
||||
void Camera::getImageSize(unsigned int & width, unsigned int & height)
|
||||
{
|
||||
width = _imageWidth;
|
||||
height = _imageHeight;
|
||||
}
|
||||
|
||||
void Camera::setCalibration(const std::string & fileName)
|
||||
{
|
||||
if(UFile::getExtension(fileName).compare("yaml") == 0)
|
||||
bool warnFrameRateTooHigh = false;
|
||||
float actualFrameRate = 0;
|
||||
if(_imageRate>0)
|
||||
{
|
||||
cv::FileStorage fs;
|
||||
fs.open(fileName, cv::FileStorage::READ);
|
||||
|
||||
if (!fs.isOpened())
|
||||
{
|
||||
UERROR("Failed to open file \"%s\"", fileName.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
cv::Mat k,d;
|
||||
|
||||
cv::FileNode n = fs["camera_matrix"];
|
||||
int rows = n["rows"];
|
||||
int cols = n["cols"];
|
||||
std::vector<double> data;
|
||||
n["data"] >> data;
|
||||
if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
|
||||
{
|
||||
k = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
|
||||
}
|
||||
|
||||
cv::FileNode nd = fs["distortion_coefficients"];
|
||||
rows = nd["rows"];
|
||||
cols = nd["cols"];
|
||||
data.clear();
|
||||
nd["data"] >> data;
|
||||
if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
|
||||
{
|
||||
d = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
|
||||
}
|
||||
|
||||
if(k.empty())
|
||||
{
|
||||
UERROR("Failed to load \"camera_matrix\" matrix.");
|
||||
}
|
||||
if(d.empty())
|
||||
{
|
||||
UERROR("Failed to load \"distortion_coefficients\" matrix.");
|
||||
}
|
||||
if(!k.empty() && !d.empty())
|
||||
{
|
||||
this->setCalibration(k, d);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("Calibration file must be in \"*.yaml\" format");
|
||||
}
|
||||
}
|
||||
|
||||
void Camera::setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients)
|
||||
{
|
||||
UASSERT(cameraMatrix.type() == CV_64FC1 &&
|
||||
cameraMatrix.rows == 3 &&
|
||||
cameraMatrix.cols == 3);
|
||||
UASSERT(distorsionCoefficients.type() == CV_64FC1 &&
|
||||
distorsionCoefficients.rows ==1 &&
|
||||
(distorsionCoefficients.cols == 4 || distorsionCoefficients.cols == 5 || distorsionCoefficients.cols == 8));
|
||||
|
||||
_k = cameraMatrix;
|
||||
_d = distorsionCoefficients;
|
||||
}
|
||||
|
||||
void Camera::resetCalibration()
|
||||
{
|
||||
_k = cv::Mat();
|
||||
_d = cv::Mat();
|
||||
}
|
||||
|
||||
cv::Mat Camera::takeImage()
|
||||
{
|
||||
cv::Mat img;
|
||||
float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
|
||||
if(imageRate>0)
|
||||
{
|
||||
int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
|
||||
int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
|
||||
if(sleepTime > 2)
|
||||
{
|
||||
uSleep(sleepTime-2);
|
||||
}
|
||||
else if(sleepTime < 0)
|
||||
{
|
||||
warnFrameRateTooHigh = true;
|
||||
actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
|
||||
}
|
||||
|
||||
// Add precision at the cost of a small overhead
|
||||
while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001)
|
||||
while(_frameRateTimer->getElapsedTime() < 1.0/double(_imageRate)-0.000001)
|
||||
{
|
||||
//
|
||||
}
|
||||
|
||||
double slept = _frameRateTimer->getElapsedTime();
|
||||
_frameRateTimer->start();
|
||||
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
|
||||
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate));
|
||||
}
|
||||
|
||||
UTimer timer;
|
||||
img = this->captureImage();
|
||||
if(!img.empty() && !_k.empty() && !_d.empty())
|
||||
SensorData data = this->captureImage();
|
||||
if(warnFrameRateTooHigh)
|
||||
{
|
||||
cv::Mat temp = img.clone();
|
||||
cv::undistort(temp, img, _k, _d);
|
||||
}
|
||||
if(!img.empty() && _mirroring)
|
||||
{
|
||||
cv::flip(img,img,1);
|
||||
}
|
||||
UDEBUG("Time capturing image = %fs", timer.ticks());
|
||||
return img;
|
||||
}
|
||||
|
||||
/////////////////////////
|
||||
// CameraImages
|
||||
/////////////////////////
|
||||
CameraImages::CameraImages(const std::string & path,
|
||||
int startAt,
|
||||
bool refreshDir,
|
||||
float imageRate,
|
||||
unsigned int imageWidth,
|
||||
unsigned int imageHeight) :
|
||||
Camera(imageRate, imageWidth, imageHeight),
|
||||
_path(path),
|
||||
_startAt(startAt),
|
||||
_refreshDir(refreshDir),
|
||||
_count(0),
|
||||
_dir(0)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
CameraImages::~CameraImages(void)
|
||||
{
|
||||
if(_dir)
|
||||
{
|
||||
delete _dir;
|
||||
}
|
||||
}
|
||||
|
||||
bool CameraImages::init()
|
||||
{
|
||||
UDEBUG("");
|
||||
if(_dir)
|
||||
{
|
||||
_dir->setPath(_path, "jpg ppm png bmp pnm tiff");
|
||||
UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
|
||||
_imageRate, actualFrameRate, timer.ticks());
|
||||
}
|
||||
else
|
||||
{
|
||||
_dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff");
|
||||
UDEBUG("Time capturing image = %fs", timer.ticks());
|
||||
}
|
||||
_count = 0;
|
||||
if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
|
||||
{
|
||||
_path.append("/");
|
||||
}
|
||||
if(!_dir->isValid())
|
||||
{
|
||||
ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
|
||||
}
|
||||
else if(_dir->getFileNames().size() == 0)
|
||||
{
|
||||
UWARN("Directory is empty \"%s\"", _path.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
|
||||
}
|
||||
return _dir->isValid();
|
||||
}
|
||||
|
||||
unsigned int CameraImages::imagesCount() const
|
||||
{
|
||||
if(_dir)
|
||||
{
|
||||
return _dir->getFileNames().size();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
cv::Mat CameraImages::captureImage()
|
||||
{
|
||||
cv::Mat img;
|
||||
UDEBUG("");
|
||||
if(_dir->isValid())
|
||||
{
|
||||
if(_refreshDir)
|
||||
{
|
||||
_dir->update();
|
||||
}
|
||||
if(_startAt == 0)
|
||||
{
|
||||
const std::list<std::string> & fileNames = _dir->getFileNames();
|
||||
if(fileNames.size())
|
||||
{
|
||||
if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
|
||||
{
|
||||
_lastFileName = *fileNames.rbegin();
|
||||
std::string fullPath = _path + _lastFileName;
|
||||
img = cv::imread(fullPath.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
std::string fileName;
|
||||
std::string fullPath;
|
||||
fileName = _dir->getNextFileName();
|
||||
if(fileName.size())
|
||||
{
|
||||
fullPath = _path + fileName;
|
||||
while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
|
||||
{
|
||||
fullPath = _path + fileName;
|
||||
}
|
||||
if(fileName.size())
|
||||
{
|
||||
ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
|
||||
|
||||
#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
|
||||
img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
|
||||
#else
|
||||
img = cv::imread(fullPath.c_str(), -1);
|
||||
#endif
|
||||
UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
|
||||
img.cols, img.rows, img.channels(), img.elemSize(), img.total());
|
||||
|
||||
#if CV_MAJOR_VERSION < 3
|
||||
// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
|
||||
if(img.depth() != CV_8U)
|
||||
{
|
||||
// The depth should be 8U
|
||||
UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
|
||||
IplImage * i = cvLoadImage(fullPath.c_str());
|
||||
img = cv::Mat(i, true);
|
||||
cvReleaseImage(&i);
|
||||
}
|
||||
#endif
|
||||
|
||||
if(img.channels()>3)
|
||||
{
|
||||
UWARN("Conversion from 4 channels to 3 channels (file=%s)", fullPath.c_str());
|
||||
cv::Mat out;
|
||||
cv::cvtColor(img, out, CV_BGRA2BGR);
|
||||
img = out;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Directory is not set, camera must be initialized.");
|
||||
}
|
||||
|
||||
unsigned int w;
|
||||
unsigned int h;
|
||||
this->getImageSize(w, h);
|
||||
|
||||
if(!img.empty() &&
|
||||
w &&
|
||||
h &&
|
||||
w != (unsigned int)img.cols &&
|
||||
h != (unsigned int)img.rows)
|
||||
{
|
||||
cv::Mat resampled;
|
||||
cv::resize(img, resampled, cv::Size(w, h));
|
||||
img = resampled;
|
||||
}
|
||||
return img;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/////////////////////////
|
||||
// CameraVideo
|
||||
/////////////////////////
|
||||
CameraVideo::CameraVideo(int usbDevice,
|
||||
float imageRate,
|
||||
unsigned int imageWidth,
|
||||
unsigned int imageHeight) :
|
||||
Camera(imageRate, imageWidth, imageHeight),
|
||||
_src(kUsbDevice),
|
||||
_usbDevice(usbDevice)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
CameraVideo::CameraVideo(const std::string & filePath,
|
||||
float imageRate,
|
||||
unsigned int imageWidth,
|
||||
unsigned int imageHeight) :
|
||||
Camera(imageRate, imageWidth, imageHeight),
|
||||
_filePath(filePath),
|
||||
_src(kVideoFile),
|
||||
_usbDevice(0)
|
||||
{
|
||||
}
|
||||
|
||||
CameraVideo::~CameraVideo()
|
||||
{
|
||||
_capture.release();
|
||||
}
|
||||
|
||||
bool CameraVideo::init()
|
||||
{
|
||||
if(_capture.isOpened())
|
||||
{
|
||||
_capture.release();
|
||||
}
|
||||
|
||||
if(_src == kUsbDevice)
|
||||
{
|
||||
unsigned int w;
|
||||
unsigned int h;
|
||||
this->getImageSize(w, h);
|
||||
|
||||
ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d with imgSize=[%d,%d]", _usbDevice, w, h);
|
||||
_capture.open(_usbDevice);
|
||||
|
||||
if(w && h)
|
||||
{
|
||||
_capture.set(CV_CAP_PROP_FRAME_WIDTH, double(w));
|
||||
_capture.set(CV_CAP_PROP_FRAME_HEIGHT, double(h));
|
||||
}
|
||||
}
|
||||
else if(_src == kVideoFile)
|
||||
{
|
||||
ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str());
|
||||
_capture.open(_filePath.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
ULOGGER_ERROR("Camera: Unknown source...");
|
||||
}
|
||||
if(!_capture.isOpened())
|
||||
{
|
||||
ULOGGER_ERROR("Camera: Failed to create a capture object!");
|
||||
_capture.release();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
cv::Mat CameraVideo::captureImage()
|
||||
{
|
||||
cv::Mat img;
|
||||
if(_capture.isOpened())
|
||||
{
|
||||
if(_capture.read(img))
|
||||
{
|
||||
unsigned int w;
|
||||
unsigned int h;
|
||||
this->getImageSize(w, h);
|
||||
|
||||
if(!img.empty() &&
|
||||
w &&
|
||||
h &&
|
||||
w != (unsigned int)img.cols &&
|
||||
h != (unsigned int)img.rows)
|
||||
{
|
||||
cv::Mat resampled;
|
||||
cv::resize(img, resampled, cv::Size(w, h));
|
||||
img = resampled;
|
||||
}
|
||||
else
|
||||
{
|
||||
// clone required
|
||||
img = img.clone();
|
||||
}
|
||||
}
|
||||
else if(_usbDevice)
|
||||
{
|
||||
UERROR("Camera has been disconnected!");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ULOGGER_WARN("The camera must be initialized before requesting an image.");
|
||||
}
|
||||
return img;
|
||||
return data;
|
||||
}
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
Reference in New Issue
Block a user