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

Added parameter Mem/RotateImagesUpsideUp

Browse Source
This commit is contained in:
matlabbe
2024-01-10 14:54:55 -08:00
parent a0559b156b
commit b7239fdc84
7 changed files with 448 additions and 215 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
corelib/src/Memory.cpp
View File

@@ -107,6 +107,7 @@ Memory::Memory(const ParametersMap & parameters) :
_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
_useOdometryGravity(Parameters::defaultMemUseOdomGravity()),
_rotateImagesUpsideUp(Parameters::defaultMemRotateImagesUpsideUp()),
_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
@@ -597,6 +598,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
Parameters::parse(params, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
Parameters::parse(params, Parameters::kMemUseOdomGravity(), _useOdometryGravity);
Parameters::parse(params, Parameters::kMemRotateImagesUpsideUp(), _rotateImagesUpsideUp);
Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
@@ -4667,6 +4669,96 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
preUpdateThread.start();
}
if(_rotateImagesUpsideUp && !data.imageRaw().empty() && !data.cameraModels().empty())
{
// Currently stereo is not supported
UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
int subInputImageWidth = data.imageRaw().cols/data.cameraModels().size();
int subInputDepthWidth = data.depthRaw().cols/data.cameraModels().size();
int subOutputImageWidth = 0;
int subOutputDepthWidth = 0;
cv::Mat rotatedColorImages;
cv::Mat rotatedDepthImages;
std::vector<CameraModel> rotatedCameraModels;
bool allOutputSizesAreOkay = true;
for(size_t i=0; i<data.cameraModels().size(); ++i)
{
UDEBUG("Rotating camera %ld", i);
cv::Mat rgb = cv::Mat(data.imageRaw(), cv::Rect(subInputImageWidth*i, 0, subInputImageWidth, data.imageRaw().rows));
cv::Mat depth = !data.depthRaw().empty()?cv::Mat(data.depthRaw(), cv::Rect(subInputDepthWidth*i, 0, subInputDepthWidth, data.depthRaw().rows)):cv::Mat();
CameraModel model = data.cameraModels()[i];
util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
if(rotatedColorImages.empty())
{
rotatedColorImages = cv::Mat(cv::Size(rgb.cols * data.cameraModels().size(), rgb.rows), rgb.type());
subOutputImageWidth = rgb.cols;;
if(!depth.empty())
{
rotatedDepthImages = cv::Mat(cv::Size(depth.cols * data.cameraModels().size(), depth.rows), depth.type());
subOutputDepthWidth = depth.cols;
}
}
else if(rgb.cols != subOutputImageWidth || depth.cols != subOutputDepthWidth ||
rgb.rows != rotatedColorImages.rows || depth.rows != rotatedDepthImages.rows)
{
UWARN("Rotated image for camera index %d (rgb=%dx%d depth=%dx%d) doesn't tally "
"with the first camera (rgb=%dx%d, depth=%dx%d). Aborting upside up rotation, "
"will use original image orientation. Set parameter %s to false to avoid "
"this warning.",
i,
rgb.cols, rgb.rows,
depth.cols, depth.rows,
subOutputImageWidth, rotatedColorImages.rows,
subOutputDepthWidth, rotatedDepthImages.rows,
Parameters::kMemRotateImagesUpsideUp().c_str());
allOutputSizesAreOkay = false;
break;
}
rgb.copyTo(cv::Mat(rotatedColorImages, cv::Rect(subOutputImageWidth*i, 0, subOutputImageWidth, rgb.rows)));
if(!depth.empty())
{
depth.copyTo(cv::Mat(rotatedDepthImages, cv::Rect(subOutputDepthWidth*i, 0, subOutputDepthWidth, depth.rows)));
}
rotatedCameraModels.push_back(model);
}
if(allOutputSizesAreOkay)
{
data.setRGBDImage(rotatedColorImages, rotatedDepthImages, rotatedCameraModels);
// Clear any features to avoid confusion with the rotated cameras.
if(!data.keypoints().empty() || !data.keypoints3D().empty() || !data.descriptors().empty())
{
if(_useOdometryFeatures)
{
static bool warned = false;
if(!warned)
{
UWARN("Because parameter %s is enabled, parameter %s is inhibited as "
"features have to be regenerated. To avoid this warning, set "
"explicitly %s to false. This message is only "
"printed once.",
Parameters::kMemRotateImagesUpsideUp().c_str(),
Parameters::kMemUseOdomFeatures().c_str(),
Parameters::kMemUseOdomFeatures().c_str());
warned = true;
}
}
data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
}
}
}
else if(_rotateImagesUpsideUp)
{
static bool warned = false;
if(!warned)
{
UWARN("Parameter %s can only be used with RGB-only or RGB-D cameras. "
"Ignoring upside up rotation. This message is only printed once.",
Parameters::kMemRotateImagesUpsideUp().c_str());
warned = true;
}
}
unsigned int preDecimation = 1;
std::vector<cv::Point3f> keypoints3D;
SensorData decimatedData;

92
corelib/src/util2d.cpp
View File

@@ -2202,6 +2202,98 @@ void NMS(
}
}
void rotateImagesUpsideUpIfNecessary(
CameraModel & model,
cv::Mat & rgb,
cv::Mat & depth)
{
float roll,pitch,yaw;
// remove optical rotation
Transform localTransform = model.localTransform()*CameraModel::opticalRotation().inverse();
localTransform.getEulerAngles(roll, pitch, yaw);
UDEBUG("roll=%f pitch=%f yaw=%f", roll, pitch, yaw);
if(fabs(pitch > M_PI/4))
{
// Return original because of ambiguity for what would be considered up...
UDEBUG("Ignoring image rotation as pitch(%f)>Pi/4", pitch);
return;
}
if(roll<0)
{
roll+=2*M_PI;
}
if(roll >= M_PI/4 && roll < 3*M_PI/4)
{
UDEBUG("ROTATION_90 (roll=%f)", roll);
if(!rgb.empty())
{
cv::flip(rgb,rgb,1);
cv::transpose(rgb,rgb);
}
if(!depth.empty())
{
cv::flip(depth,depth,1);
cv::transpose(depth,depth);
}
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
model.fx(),
model.cy(),
model.cx()>0?model.imageWidth()-model.cx():0,
model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
}
else if(roll >= 3*M_PI/4 && roll < 5*M_PI/4)
{
UDEBUG("ROTATION_180 (roll=%f)", roll);
if(!rgb.empty())
{
cv::flip(rgb,rgb,1);
cv::flip(rgb,rgb,0);
}
if(!depth.empty())
{
cv::flip(depth,depth,1);
cv::flip(depth,depth,0);
}
cv::Size sizet(model.imageWidth(), model.imageHeight());
model = CameraModel(
model.fx(),
model.fy(),
model.cx()>0?model.imageWidth()-model.cx():0,
model.cy()>0?model.imageHeight()-model.cy():0,
model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
model.setImageSize(sizet);
}
else if(roll >= 5*M_PI/4 && roll < 7*M_PI/4)
{
UDEBUG("ROTATION_270 (roll=%f)", roll);
if(!rgb.empty())
{
cv::transpose(rgb,rgb);
cv::flip(rgb,rgb,1);
}
if(!depth.empty())
{
cv::transpose(depth,depth);
cv::flip(depth,depth,1);
}
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
model.fx(),
model.cy()>0?model.imageHeight()-model.cy():0,
model.cx(),
model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
}
else
{
UDEBUG("ROTATION_0 (roll=%f)", roll);
}
}
}
}