Commit 09fdfdfd authored by Rafael Mosberger's avatar Rafael Mosberger

updates

parent 9ba5f380
......@@ -148,7 +148,7 @@ include_directories(
# add_executable(${PROJECT_NAME}_node src/test_node.cpp)
add_executable(reflex_ros_node src/reflex_driver.cpp)
target_link_libraries(reflex_ros_node reflex_application reflex_driver_dev reflex_base ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
target_link_libraries(reflex_ros_node reflex_application reflex_driver_opencv reflex_base ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
add_dependencies(reflex_ros_node ${PROJECT_NAME}_generate_messages_cpp)
## Rename C++ executable without prefix
......
......@@ -2,10 +2,12 @@
#define REFLEX_CAMERA_H
#include <reflex/base/FileAccessObject.hpp>
#include <reflex/driver/ReflexDriverDev.hpp>
#include <reflex/camera/CameraIntrinsics.hpp>
#include <reflex/camera/CameraExtrinsics.hpp>
#include <reflex/camera/ImageRectifier.hpp>
#include <reflex/driver/ReflexDriverCV.hpp>
#include <reflex/calibration/CameraIntrinsics.hpp>
#include <reflex/calibration/CameraExtrinsics.hpp>
#include <reflex/calibration/StereoImageRectifier.hpp>
#include <reflex/calibration/UndistortionParameters.hpp>
#include <reflex/calibration/StereoRectificationParameters.hpp>
#include <reflex/camera/SensorConfig.hpp>
#include <reflex/system/UnitInfo.hpp>
#include <reflex/system/UnitGeometry.hpp>
......@@ -27,31 +29,27 @@ public:
virtual ~CameraSensor();
CameraIntrinsics intrinsics;
RectificationParameters rectification;
ImageRectifier rectifier;
SensorConfig sensorConfig;
UndistortionParameters undistortion;
};
ReflexCamera();
ReflexCamera(const string& ipAddress, const string& serialNumber = string(""));
ReflexCamera(const string& ipAddress);
virtual ~ReflexCamera();
virtual void write(cv::FileStorage& fs) const;
virtual void read(const cv::FileNode& node);
int getNbSystems() const { return this->unitInfo.systems.size(); }
int getNbSensors() const { return this->unitInfo.sensors.size(); }
int init(const string& dataFolder = "/tmp/");
bool isInitialized() const { return _initialized; }
int loadCalibrationData(const string& dataFolder);
int loadRectifier();
int reboot();
int changeOperationState(int state, float timeout = 5.);
int changeNetworkSettings(const std::string& ipAddress, const std::string& netmask = "255.255.255.0", const std::string& gateway = "");
int changeSensorConfiguration(const std::string& name);
int changeSensorMode(int sensorID, int8_t mode);
int changeSensorShutter(int sensorID, uint32_t shutter);
int changeSensorGain(int sensorID, float gain);
......@@ -61,7 +59,7 @@ public:
int receiveImage(int sensorID, ReflexImageCV& image) const;
int receiveImages(const std::vector<int>& sensorID, std::vector<ReflexImageCV>& images) const;
int receiveImagesFull(std::vector<ReflexImageCV>& images) const;
int receiveImages(const std::vector<int>& sensorIDs, std::vector<ReflexImageCV>& imagesSig, std::vector<ReflexImageCV>& imagesRef) const;
int receiveDataAndImage(ReflexData& data, int sensorID, ReflexImageCV& image) const;
int receiveDataAndImages(ReflexData& data, const std::vector<int>& sensorID, std::vector<ReflexImageCV>& image) const;
......@@ -73,30 +71,37 @@ public:
int receiveSensorStatus(std::vector<ReflexSensorStatus>& status) const;
int receiveCameraPose(std::vector<double>& pose) const;
int changeCameraPose(const std::vector<double>& pose, float timeout = -1.) const;
int changeCameraPose(const std::vector<double>& pose, float timeout = -1.);
int undistortImage(const cv::Mat& imgInput, int sensorID, cv::Mat& imgOutput) const;
int rectifyImage(const cv::Mat& imgInput, int sensorPairID, int sensorID, cv::Mat& imgOutput) const;
template<class Tp>
int rectifyPoints(const std::vector<cv::Point_<Tp>>& pointsRaw, int sensorPairID, int sensorID, std::vector<cv::Point_<Tp>>& pointsRectified) const;
string getDataFolder() const { return _dataFolder; }
static string getTypeName(int type);
string getTypeName() const { return getTypeName(info.type); }
static int getNIRSystemID(int unitType);
static int getRGBSystemID(int unitType);
int unitType;
ReflexCameraUnit info;
int operationState;
Pose3d pose;
string ipAddress;
string serialNumber;
int operationState;
string softwareVersion;
int softwareVersionMajor;
int softwareVersionMinor;
int softwareVersionPatch;
string ipAddress;
UnitInfo unitInfo;
Environment environment;
vector<vector<CameraExtrinsics>> extrinsics;
Environment environment;
vector<CameraExtrinsics> extrinsics;
vector<StereoRectificationParameters> stereoRectification;
vector<StereoImageRectifier> stereoRectifier;
vector<CameraSensor> cameras;
vector<CameraSensor> cameras;
protected:
......@@ -105,6 +110,17 @@ protected:
};
template<class Tp>
int ReflexCamera::rectifyPoints(const std::vector<cv::Point_<Tp>>& pointsRaw, int sensorPairID, int sensorID, std::vector<cv::Point_<Tp>>& pointsRectified) const
{
if(sensorPairID >= (int)stereoRectifier.size())
return ERROR_INITIALIZATION;
return this->stereoRectifier[sensorPairID].rectifyPoints(sensorID, pointsRaw, pointsRectified);
}
} // end namespace reflex
......
......@@ -46,7 +46,11 @@ protected:
if(node.empty())
return false;
node >> value;
node >> value; // if T is a cv::Ptr
// this will call cv::read(const FileNode& node, reflex::FileAccessObject* obj, const reflex::FileAccessObject* default
// otherwise:
// this will call cv::read(const cv::FileNode& nh, T& value, const T&)
return true;
}
......@@ -72,7 +76,10 @@ protected:
template <class T>
static inline void saveField(cv::FileStorage& fs, string key, T& value)
{
fs << key << value;
fs << key << value; // if T is a cv::Ptr:
// this will call cv::write(cv::FileStorage& fs, const cv::String& key, const reflex::FileAccessObject* obj)
// otherwise
// this will call cv::write(cv::FileStorage& fs, const cv::String& key, T value)
}
......@@ -94,7 +101,7 @@ protected:
namespace cv
{
void write(cv::FileStorage& fs, const string&, const reflex::FileAccessObject* obj);
void write(cv::FileStorage& fs, const cv::String& key, const reflex::FileAccessObject* obj);
void read(const FileNode& node, reflex::FileAccessObject* obj, const reflex::FileAccessObject* defaultValue = NULL);
}
......
......@@ -17,18 +17,40 @@ public:
virtual void write(cv::FileStorage& fs) const;
virtual void read(const cv::FileNode& node);
CameraExtrinsics getInverse() const;
bool isEmpty() const;
cv::Mat R;
cv::Mat R; // rotation matrix between first and second camera
cv::Vec3d rvec; // rotation vector (rodrigues transform) corresponding to matrix R
cv::Mat T;
std::string camSerialNumber;
int calibSerialNumber;
cv::Mat T; // translation vector between the coordinate systems of the cameras
cv::Mat E; // essential matrix
cv::Mat F; // fundamental matrix
};
std::string cameraUnitSerialNumber;
int calibrationSerialNumber;
};
} // end namespace reflex
namespace cv
{
inline void write(cv::FileStorage& fs, const cv::String& key, reflex::CameraExtrinsics value)
{
fs << "{";
value.write(fs);
fs << "}";
}
inline void read(const cv::FileNode& nh, reflex::CameraExtrinsics& value, const reflex::CameraExtrinsics&)
{
value.read(nh);
}
}
#endif
......@@ -20,8 +20,12 @@ public:
CameraIntrinsics convert(const cv::Size& subsampling, const cv::Point& roiPos, const cv::Size& roiSize) const;
CameraIntrinsics clone() const;
bool isEmpty() const;
bool isEqual(const CameraIntrinsics& intrinsics);
void projectPointsToImage(cv::InputArray objectPoints, cv::OutputArray imagePoints, cv::InputArray rvec = cv::noArray(), cv::InputArray tvec = cv::noArray()) const;
cv::Point2d getCenter() const { return cv::Point2d(cameraMatrix.at<double>(0,2), cameraMatrix.at<double>(1,2)); }
void computeCharacteristics();
......@@ -29,8 +33,8 @@ public:
cv::Size imageSize;
cv::Mat cameraMatrix;
cv::Mat distCoeffs;
std::string camSerialNumber;
int calibSerialNumber;
std::string cameraUnitSerialNumber;
int calibrationSerialNumber;
cv::Point2d aperture;
cv::Point2d fov;
......@@ -43,4 +47,24 @@ public:
} // end namespace reflex
namespace cv
{
inline void write(cv::FileStorage& fs, const cv::String& key, reflex::CameraIntrinsics value)
{
fs << "{";
value.write(fs);
fs << "}";
}
inline void read(const cv::FileNode& nh, reflex::CameraIntrinsics& value, const reflex::CameraIntrinsics&)
{
value.read(nh);
}
}
#endif
......@@ -3,7 +3,6 @@
#include <reflex/base/FileAccessObject.hpp>
namespace reflex
{
......@@ -22,8 +21,8 @@ public:
cv::Mat map[2]; // two tables for x and y coordinates
int format; // data format of the remapping tables (CV_16S2 or CV_32FC1)
std::string camSerialNumber;
int calibSerialNumber;
std::string cameraUnitSerialNumber;
int calibrationSerialNumber;
};
......
#ifndef IMAGE_RECTIFIER_H
#define IMAGE_RECTIFIER_H
#ifndef STEREO_IMAGE_RECTIFIER_H
#define STEREO_IMAGE_RECTIFIER_H
#include <reflex/base/FileAccessObject.hpp>
#include <reflex/camera/RectificationParameters.hpp>
#include <reflex/camera/CameraIntrinsics.hpp>
#include "StereoRectificationParameters.hpp"
namespace reflex
{
class ImageRectifier : public FileAccessObject
class StereoImageRectifier : public FileAccessObject
{
public:
ImageRectifier();
ImageRectifier(const string& fnIntrinsics, const string& fnRectification);
virtual ~ImageRectifier();
StereoImageRectifier();
virtual ~StereoImageRectifier();
virtual void write(cv::FileStorage& fs) const;
virtual void read(const cv::FileNode& node);
int loadParams(const string& fnIntrinsics, const string& fnRectification);
int loadParams(const string& fnRectification, bool initMaps = false);
int rectifyImage(const cv::Mat imgRaw, cv::Mat& imgRectified, int interpolation = cv::INTER_LINEAR, int borderMode = cv::BORDER_CONSTANT, const cv::Scalar& borderValue = cv::Scalar(127)) const;
int rectifyImage(int sensorID, const cv::Mat imgRaw, cv::Mat& imgRectified, int interpolation = cv::INTER_LINEAR, int borderMode = cv::BORDER_CONSTANT, const cv::Scalar& borderValue = cv::Scalar(0)) const;
int rectifyRows(const cv::Mat imgRaw, cv::Mat& imgRectified, const cv::Mat& imgMask, int interpolation = cv::INTER_LINEAR, int borderMode = cv::BORDER_CONSTANT, const cv::Scalar& borderValue = cv::Scalar(127)) const;
int rectifyRows(int sensorID, const cv::Mat imgRaw, cv::Mat& imgRectified, const cv::Mat& imgMask, int interpolation = cv::INTER_LINEAR, int borderMode = cv::BORDER_CONSTANT, const cv::Scalar& borderValue = cv::Scalar(0)) const;
int rectifyRois(const cv::Mat imgRaw, cv::Mat& imgRectified, const std::vector<cv::Rect>& rois, int interpolation = cv::INTER_LINEAR, int borderMode = cv::BORDER_CONSTANT, const cv::Scalar& borderValue = cv::Scalar(127)) const;
int rectifyRoi(const cv::Mat imgRaw, cv::Mat& imgRectified, const cv::Rect& roi, int interpolation = cv::INTER_LINEAR, int borderMode = cv::BORDER_CONSTANT, const cv::Scalar& borderValue = cv::Scalar(127)) const;
int rectifyRois(int sensorID, const cv::Mat imgRaw, cv::Mat& imgRectified, const std::vector<cv::Rect>& rois, int interpolation = cv::INTER_LINEAR, int borderMode = cv::BORDER_CONSTANT, const cv::Scalar& borderValue = cv::Scalar(0)) const;
int rectifyRoi(int sensorID, const cv::Mat imgRaw, cv::Mat& imgRectified, const cv::Rect& roi, int interpolation = cv::INTER_LINEAR, int borderMode = cv::BORDER_CONSTANT, const cv::Scalar& borderValue = cv::Scalar(0)) const;
template<class Tp>
int rectifyPoints(const std::vector<cv::Point_<Tp>>& pointsRaw, std::vector<cv::Point_<Tp>>& pointsRectified) const;
int rectifyPoints(int sensorID, const std::vector<cv::Point_<Tp>>& pointsRaw, std::vector<cv::Point_<Tp>>& pointsRectified) const;
template<class Tp>
void projectTo3D(const cv::Point_<Tp>& pt2D, cv::Point3_<Tp>& pt3D) const;
void projectTo3D(int sensorID, const cv::Point_<Tp>& pt2D, cv::Point3_<Tp>& pt3D) const;
template<class Tp>
void projectTo3D(const cv::Vec<Tp,2>& pt2D, cv::Vec<Tp,3>& pt3D) const;
void projectTo3D(int sensorID, const cv::Vec<Tp,2>& pt2D, cv::Vec<Tp,3>& pt3D) const;
template<class Tp>
void projectTo3D(const cv::Point_<Tp>& pt2D, Tp disparity, cv::Point3_<Tp>& pt3D) const;
void projectTo3D(int sensorID, const cv::Point_<Tp>& pt2D, Tp disparity, cv::Point3_<Tp>& pt3D) const;
template<class Tp>
void projectTo3D(const cv::Vec<Tp,2>& pt2D, Tp disparity, cv::Vec<Tp,3>& pt3D) const;
void projectTo3D(int sensorID, const cv::Vec<Tp,2>& pt2D, Tp disparity, cv::Vec<Tp,3>& pt3D) const;
CameraIntrinsics intrinsics;
RectificationParameters params;
StereoRectificationParameters params;
};
template<class Tp>
int ImageRectifier::rectifyPoints(const std::vector<cv::Point_<Tp>>& pointsRaw, std::vector<cv::Point_<Tp>>& pointsRectified) const
int StereoImageRectifier::rectifyPoints(int sensorID, const std::vector<cv::Point_<Tp>>& pointsRaw, std::vector<cv::Point_<Tp>>& pointsRectified) const
{
if(params.map.empty())
if(params.map[sensorID].empty())
return ERROR_INITIALIZATION;
if(typeid(Tp) == typeid(int))
{
std::vector<cv::Point2f> pointsRaw2f(pointsRaw.begin(), pointsRaw.end());
std::vector<cv::Point2f> pointsRectified2f;
cv::undistortPoints(pointsRaw2f, pointsRectified2f, intrinsics.cameraMatrix, intrinsics.distCoeffs, params.R, params.P);
cv::undistortPoints(pointsRaw2f, pointsRectified2f, params.origCameraMatrix[sensorID], params.origDistCoeffs[sensorID], params.R[sensorID], params.P[sensorID]);
pointsRectified = std::vector<cv::Point_<Tp>>(pointsRectified2f.begin(), pointsRectified2f.end());
}
else
cv::undistortPoints(pointsRaw, pointsRectified, intrinsics.cameraMatrix, intrinsics.distCoeffs, params.R, params.P);
cv::undistortPoints(pointsRaw, pointsRectified, params.origCameraMatrix[sensorID], params.origDistCoeffs[sensorID], params.R[sensorID], params.P[sensorID]);
return ERROR_NONE;
}
template<class Tp>
void ImageRectifier::projectTo3D(const cv::Point_<Tp>& pt2D, cv::Point3_<Tp>& pt3D) const
void StereoImageRectifier::projectTo3D(int sensorID, const cv::Point_<Tp>& pt2D, cv::Point3_<Tp>& pt3D) const
{
pt3D.x = (pt2D.x + params.q03) / params.q23;
pt3D.y = (pt2D.y + params.q13) / params.q23;
......@@ -80,7 +76,7 @@ void ImageRectifier::projectTo3D(const cv::Point_<Tp>& pt2D, cv::Point3_<Tp>& pt
template<class Tp>
void ImageRectifier::projectTo3D(const cv::Vec<Tp,2>& pt2D, cv::Vec<Tp,3>& pt3D) const
void StereoImageRectifier::projectTo3D(int sensorID, const cv::Vec<Tp,2>& pt2D, cv::Vec<Tp,3>& pt3D) const
{
pt3D[X] = (pt2D.x + params.q03) / params.q23;
pt3D[Y] = (pt2D.y + params.q13) / params.q23;
......@@ -89,7 +85,7 @@ void ImageRectifier::projectTo3D(const cv::Vec<Tp,2>& pt2D, cv::Vec<Tp,3>& pt3D)
template<class Tp>
void ImageRectifier::projectTo3D(const cv::Point_<Tp>& pt2D, Tp disparity, cv::Point3_<Tp>& pt3D) const
void StereoImageRectifier::projectTo3D(int sensorID, const cv::Point_<Tp>& pt2D, Tp disparity, cv::Point3_<Tp>& pt3D) const
{
const Tp pw = (Tp)(1.0f / (disparity * params.q32 + params.q33));
pt3D.x = (pt2D.x + params.q03) * pw;
......@@ -99,7 +95,7 @@ void ImageRectifier::projectTo3D(const cv::Point_<Tp>& pt2D, Tp disparity, cv::P
template<class Tp>
void ImageRectifier::projectTo3D(const cv::Vec<Tp,2>& pt2D, Tp disparity, cv::Vec<Tp,3>& pt3D) const
void StereoImageRectifier::projectTo3D(int sensorID, const cv::Vec<Tp,2>& pt2D, Tp disparity, cv::Vec<Tp,3>& pt3D) const
{
const Tp pw = (Tp)(1.0f / (disparity * params.q32 + params.q33));
pt3D[X] = (pt2D.x + params.q03) * pw;
......
#ifndef STEREO_RECTIFICATION_PARAMETERS_H
#define STEREO_RECTIFICATION_PARAMETERS_H
#include <reflex/base/FileAccessObject.hpp>
#include "RectificationMap.hpp"
namespace reflex
{
class StereoRectificationParameters : public FileAccessObject
{
public:
StereoRectificationParameters();
virtual ~StereoRectificationParameters();
virtual void write(cv::FileStorage& fs) const;
virtual void read(const cv::FileNode& node);
int initMaps();
int loadMaps();
int saveMaps();
std::string cameraUnitSerialNumber;
int calibrationSerialNumber;
cv::Mat R[2];
cv::Vec3d rvec[2]; // rotation vector (rodrigues transform) corresponding to matrix R
cv::Mat P[2];
cv::Mat Q;
cv::Mat Q32;
double q03, q13, q23, q32, q33;
RectificationMap map[2];
cv::Size imageSize;
cv::Rect validRoi[2]; // ROIs delimiting the area in which valid (defined) pixels are found
cv::Mat origCameraMatrix[2];
cv::Mat origDistCoeffs[2];
int m1type;
protected:
};
} // end namespace reflex
#endif
#ifndef UNDISTORTION_PARAMETERS_H
#define UNDISTORTION_PARAMETERS_H
#include <reflex/base/FileAccessObject.hpp>
#include "RectificationMap.hpp"
namespace reflex
{
class UndistortionParameters : public FileAccessObject
{
public:
UndistortionParameters();
UndistortionParameters(const string& fn);
virtual ~UndistortionParameters();
virtual void write(cv::FileStorage& fs) const;
virtual void read(const cv::FileNode& node);
int initMap();
int loadMap();
int saveMap();
std::string cameraUnitSerialNumber;
int calibrationSerialNumber;
cv::Mat cameraMatrix;
cv::Size imageSize;
cv::Rect validRoi;
RectificationMap map;
cv::Mat origCameraMatrix;
cv::Mat origDistCoeffs;
cv::Mat R;
int m1type;
/*
* InputArray cameraMatrix,
InputArray distCoeffs,
InputArray R,
int m1type,
*/
protected:
};
} // end namespace reflex
#endif
#ifndef RECTIFICATION_PARAMETERS_H
#define RECTIFICATION_PARAMETERS_H
#include <reflex/base/FileAccessObject.hpp>
#include <reflex/camera/RectificationMap.hpp>
namespace reflex{
class RectificationParameters : public FileAccessObject
{
public:
RectificationParameters();
RectificationParameters(const string& fn);
virtual ~RectificationParameters();
virtual void write(cv::FileStorage& fs) const;
virtual void read(const cv::FileNode& node);
int loadRectificationMap();
void computeRectificationMap(const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs);
int saveRectificationMap();
cv::Mat R;
cv::Vec3d rvec; // rotation vector (rodrigues transform) corresponding to matrix R
cv::Mat P;
cv::Mat Q;
cv::Mat Q32;
double q03, q13, q23, q32, q33;
cv::Size imageSize;
double scale;
int flags;
RectificationMap map;
cv::Rect validRoi; // ROIs delimiting the area in which valid (defined) pixels are found
std::string camSerialNumber;
int calibSerialNumber;
protected:
};
} // end namespace reflex
#endif
......@@ -5,7 +5,7 @@
#include <reflex/driver/ReflexDriver.hpp>
#include <reflex/detector/Reflector.hpp>
#include <reflex/geometry/Pose3D.hpp>
#include <reflex/camera/CameraIntrinsics.hpp>
#include <reflex/calibration/CameraIntrinsics.hpp>
namespace reflex
......@@ -15,9 +15,24 @@ class Drawing
{
public:
static void drawCross(cv::Mat &img, const cv::Point& point, cv::Scalar color, int thickness = 1, int size = 3, int lineType = cv::LINE_8, int shift = 0);
template <class T>
static void drawCross(cv::Mat &img, const cv::Point_<T>& point, cv::Scalar color, int thickness = 1, T size = 3., int lineType = cv::LINE_8, int shift = 0);
template <class T>
static void drawLine(cv::Mat& img, const cv::Point_<T>& point1, const cv::Point_<T>& point2, const cv::Scalar& color, int thickness = 1, int lineType = cv::LINE_8, int shift = 0);
template <class T>
static void drawCircles(cv::Mat& img, const std::vector<cv::Point_<T>>& points, int radius, const cv::Scalar& color