i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

RegVis: if calibration is not found, local features matching using guess is not done, global matching is done instead. CameraModel/StereoCameraModel: added imageSize argument to constructors

Browse Source
This commit is contained in:
matlabbe
2016-03-09 17:15:05 -05:00
parent 49b5514319
commit 0f477d6853
5 changed files with 41 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/include/rtabmap/core/CameraModel.h
View File

@@ -59,7 +59,8 @@ public:
double cx,
double cy,
const Transform & localTransform = Transform::getIdentity(),
double Tx = 0.0f);
double Tx = 0.0f,
const cv::Size & imageSize = cv::Size(0,0));
// minimal to be saved
CameraModel(
const std::string & name,
@@ -68,7 +69,8 @@ public:
double cx,
double cy,
const Transform & localTransform = Transform::getIdentity(),
double Tx = 0.0f);
double Tx = 0.0f,
const cv::Size & imageSize = cv::Size(0,0));
virtual ~CameraModel() {}

6
corelib/include/rtabmap/core/StereoCameraModel.h
View File

@@ -68,7 +68,8 @@ public:
double cx,
double cy,
double baseline,
const Transform & localTransform = Transform::getIdentity());
const Transform & localTransform = Transform::getIdentity(),
const cv::Size & imageSize = cv::Size(0,0));
//minimal to be saved
StereoCameraModel(
const std::string & name,
@@ -77,7 +78,8 @@ public:
double cx,
double cy,
double baseline,
const Transform & localTransform = Transform::getIdentity());
const Transform & localTransform = Transform::getIdentity(),
const cv::Size & imageSize = cv::Size(0,0));
virtual ~StereoCameraModel() {}
bool isValidForProjection() const {return left_.isValidForProjection() && right_.isValidForProjection() && baseline() > 0.0;}

8
corelib/src/CameraModel.cpp
View File

@@ -67,7 +67,9 @@ CameraModel::CameraModel(
double cx,
double cy,
const Transform & localTransform,
double Tx) :
double Tx,
const cv::Size & imageSize) :
imageSize_(imageSize),
K_(cv::Mat::eye(3, 3, CV_64FC1)),
localTransform_(localTransform)
{
@@ -99,8 +101,10 @@ CameraModel::CameraModel(
double cx,
double cy,
const Transform & localTransform,
double Tx) :
double Tx,
const cv::Size & imageSize) :
name_(name),
imageSize_(imageSize),
K_(cv::Mat::eye(3, 3, CV_64FC1)),
localTransform_(localTransform)
{

22
corelib/src/RegistrationVis.cpp
View File

@@ -540,9 +540,18 @@ Transform RegistrationVis::computeTransformationImpl(
// We have all data we need here, so match!
if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
{
cv::Size imageSize = toSignature.sensorData().imageRaw().size();
bool isCalibrated = false;
if(imageSize.height == 0 || imageSize.width == 0)
{
imageSize = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModel().left().imageSize();
}
isCalibrated = imageSize.height != 0 && imageSize.width != 0 && fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].isValidForProjection():fromSignature.sensorData().stereoCameraModel().isValidForProjection();
// If guess is set, limit the search of matches using optical flow window size
bool guessSet = !guess.isIdentity() && !guess.isNull();
if(guessSet && _guessWinSize > 0 && kptsFrom3D.size())
if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() &&
isCalibrated) // needed for projection
{
UDEBUG("");
UASSERT((int)kptsTo.size() == descriptorsTo.rows);
@@ -573,8 +582,8 @@ Transform RegistrationVis::computeTransformationImpl(
int oi=0;
for(unsigned int i=0; i<projected.size(); ++i)
{
if(uIsInBounds(projected[i].x, 0.0f, float(toSignature.sensorData().imageRaw().cols-1)) &&
uIsInBounds(projected[i].y, 0.0f, float(toSignature.sensorData().imageRaw().rows-1)))
if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)))
{
projectedIndexToDescIndex[oi] = i;
cornersProjected[oi++] = projected[i];
@@ -755,6 +764,13 @@ Transform RegistrationVis::computeTransformationImpl(
}
else
{
if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() && !isCalibrated)
{
UWARN("Calibration not found! Finding correspondences "
"with the guess cannot be done, global matching is "
"done instead.");
}
UDEBUG("");
// match between all descriptors
VWDictionary dictionary(_featureParameters);

14
corelib/src/StereoCameraModel.cpp
View File

@@ -130,9 +130,10 @@ StereoCameraModel::StereoCameraModel(
double cx,
double cy,
double baseline,
const Transform & localTransform) :
left_(fx, fy, cx, cy, localTransform),
right_(fx, fy, cx, cy, localTransform, baseline*-fx)
const Transform & localTransform,
const cv::Size & imageSize) :
left_(fx, fy, cx, cy, localTransform, 0, imageSize),
right_(fx, fy, cx, cy, localTransform, baseline*-fx, imageSize)
{
}
@@ -144,9 +145,10 @@ StereoCameraModel::StereoCameraModel(
double cx,
double cy,
double baseline,
const Transform & localTransform) :
left_(name+"_left", fx, fy, cx, cy, localTransform),
right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
const Transform & localTransform,
const cv::Size & imageSize) :
left_(name+"_left", fx, fy, cx, cy, localTransform, 0, imageSize),
right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx, imageSize),
name_(name)
{
}