i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-12 21:09:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

Stereo exposure compensation (#1795)

Browse Source 
* Moved stereo expsorure compensation to its own global function

* bump version

* some stats
This commit is contained in:
matlabbe
2026-10-11 16:55:38 -07:00
committed by GitHub
parent dca24fc11f
commit cf4dcab118
7 changed files with 148 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -22,7 +22,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 24)
SET(RTABMAP_PATCH_VERSION 1)
SET(RTABMAP_PATCH_VERSION 2)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
corelib/include/rtabmap/core/util2d.h
+23
View File
@@ -680,6 +680,29 @@ cv::Mat RTABMAP_CORE_EXPORT brightnessAndContrastAuto(
cv::Mat RTABMAP_CORE_EXPORT exposureFusion(
const std::vector<cv::Mat> & images);
/**
* @brief Equalizes the exposure of the left and right images of a stereo camera.
*
* Their gains are estimated with cv::detail::GainCompensator, assuming both images see
* the same scene, and applied to each one. Stereo correspondences (e.g., by optical
* flow) assume the same brightness in both images: with cameras exposing differently,
* most of them would be rejected.
*
* @param left Left image (8 bits, mono or color).
* @param right Right image (8 bits, mono or color), of the same size.
* @param leftOut Compensated left image (can be the same as @p left).
* @param rightOut Compensated right image (can be the same as @p right).
* @param gains If not null, the gains applied to the left and right images.
* @return false (and the images copied as they are) if not compensated: images of
* different sizes or not 8 bits, or OpenCV version under 3.
*/
bool RTABMAP_CORE_EXPORT stereoExposureCompensation(
const cv::Mat & left,
const cv::Mat & right,
cv::Mat & leftOut,
cv::Mat & rightOut,
std::vector<double> * gains = 0);
/**
* @brief Converts a color from HSV (Hue, Saturation, Value) to RGB.
*
corelib/src/OdometryInfo.cpp
+6
View File
@@ -187,6 +187,12 @@ std::map<std::string, float> OdometryInfo::statistics(const Transform & pose)
stats.insert(std::make_pair("Odometry/Vyaw/degps", (yaw*180.0/CV_PI)/interval));
}
}
if(!guess.isNull() && !transform.isNull())
{
// How far the motion estimated is from the guess it started from
stats.insert(std::make_pair("Odometry/Guess_error_lin/m", guess.getDistance(transform)));
stats.insert(std::make_pair("Odometry/Guess_error_ang/deg", guess.getAngle(transform)*180.0/CV_PI));
}
if(!transformGroundTruth.isNull())
{
if(!transform.isNull())
corelib/src/SensorCaptureThread.cpp
+6 -19
View File
@@ -39,7 +39,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/IMUFilter.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include <opencv2/stitching/detail/exposure_compensate.hpp>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
@@ -831,27 +830,15 @@ void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * i
{
if(data.stereoCameraModels().size()==1)
{
#if CV_MAJOR_VERSION < 3
UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.");
#else
UDEBUG("");
UTimer timer;
cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
std::vector<cv::UMat> images;
std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1, cv::Scalar(255)));
images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
compensator->feed(topLeftCorners, images, masks);
cv::Mat imgLeft = data.imageRaw().clone();
compensator->apply(0, cv::Point(0,0), imgLeft, masks[0]);
cv::Mat imgRight = data.rightRaw().clone();
compensator->apply(1, cv::Point(0,0), imgRight, masks[1]);
data.setStereoImage(imgLeft, imgRight, data.stereoCameraModels()[0]);
cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
cv::Mat imgLeft;
cv::Mat imgRight;
if(util2d::stereoExposureCompensation(data.imageRaw(), data.rightRaw(), imgLeft, imgRight))
{
data.setStereoImage(imgLeft, imgRight, data.stereoCameraModels()[0]);
}
if(info) info->timeStereoExposureCompensation = timer.ticks();
#endif
}
else
{
corelib/src/util2d.cpp
+61
View File
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#if CV_MAJOR_VERSION >= 3
#include <opencv2/photo/photo.hpp>
#include <opencv2/stitching/detail/exposure_compensate.hpp>
#endif
#if CV_MAJOR_VERSION < 5
@@ -2139,6 +2140,66 @@ cv::Mat exposureFusion(const std::vector<cv::Mat> & images)
return fusion;
}
bool stereoExposureCompensation(
const cv::Mat & left,
const cv::Mat & right,
cv::Mat & leftOut,
cv::Mat & rightOut,
std::vector<double> * gains)
{
if(gains)
{
gains->clear();
}
#if CV_MAJOR_VERSION >= 3
if(!left.empty() && left.size() == right.size() &&
left.depth() == CV_8U && right.depth() == CV_8U)
{
// The gains are estimated on images of the same type (e.g., a color left image
// with a mono right one, as stereo odometry can keep), then applied to the images
// as they are.
cv::Mat leftFeed = left;
cv::Mat rightFeed = right;
if(left.type() != right.type())
{
if(left.channels() == 3)
{
cv::cvtColor(left, leftFeed, cv::COLOR_BGR2GRAY);
}
if(right.channels() == 3)
{
cv::cvtColor(right, rightFeed, cv::COLOR_BGR2GRAY);
}
}
cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
std::vector<cv::UMat> images;
std::vector<cv::UMat> masks(2, cv::UMat(left.size(), CV_8UC1, cv::Scalar(255)));
images.push_back(leftFeed.getUMat(cv::ACCESS_READ));
images.push_back(rightFeed.getUMat(cv::ACCESS_READ));
compensator->feed(topLeftCorners, images, masks);
leftOut = left.clone();
compensator->apply(0, cv::Point(0,0), leftOut, masks[0]);
rightOut = right.clone();
compensator->apply(1, cv::Point(0,0), rightOut, masks[1]);
cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
if(gains)
{
*gains = gainCompensator->gains();
}
return true;
}
UWARN("Stereo exposure compensation needs two 8 bits images of the same size (left=%dx%d type=%d, right=%dx%d type=%d)",
left.cols, left.rows, left.type(), right.cols, right.rows, right.type());
#else
UWARN("Stereo exposure compensation is only available when rtabmap is built with OpenCV3.");
#endif
leftOut = left.clone();
rightOut = right.clone();
return false;
}
void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
{
int i;
corelib/test/test_util2d.cpp
+50 -1
View File
@@ -1440,4 +1440,53 @@ TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryPitchTooHighShouldSkip) {
EXPECT_NEAR(roll, 0.0f, 1e-5);
EXPECT_NEAR(pitch, 0.0f, 1e-5);
EXPECT_NEAR(yaw, 0.0f, 1e-5);
}
}
TEST(Util2dTest, StereoExposureCompensationBringsTheRightImageCloserToTheLeft)
{
// The same textured scene, the right camera exposing at half the left's.
cv::Mat left(120, 160, CV_8UC1);
cv::randu(left, cv::Scalar(60), cv::Scalar(200));
cv::Mat right;
left.convertTo(right, CV_8UC1, 0.5);
cv::Mat leftOut, rightOut;
std::vector<double> gains;
ASSERT_TRUE(util2d::stereoExposureCompensation(left, right, leftOut, rightOut, &gains));
ASSERT_EQ(gains.size(), 2u);
EXPECT_LT(gains[0], 1.0) << "the brighter image is darkened";
EXPECT_GT(gains[1], 1.0) << "the darker image is brightened";
const double before = cv::mean(left)[0] - cv::mean(right)[0];
const double after = cv::mean(leftOut)[0] - cv::mean(rightOut)[0];
EXPECT_LT(std::fabs(after), 0.5*std::fabs(before));
cv::Mat rightAgain;
left.convertTo(rightAgain, CV_8UC1, 0.5);
EXPECT_EQ(cv::norm(right, rightAgain, cv::NORM_INF), 0.0) << "the inputs are not modified";
}
TEST(Util2dTest, StereoExposureCompensationKeepsAColorLeftImage)
{
cv::Mat left(120, 160, CV_8UC3);
cv::randu(left, cv::Scalar::all(60), cv::Scalar::all(200));
cv::Mat right;
cv::cvtColor(left, right, cv::COLOR_BGR2GRAY);
right.convertTo(right, CV_8UC1, 0.5);
cv::Mat leftOut, rightOut;
ASSERT_TRUE(util2d::stereoExposureCompensation(left, right, leftOut, rightOut));
EXPECT_EQ(leftOut.type(), CV_8UC3);
EXPECT_EQ(rightOut.type(), CV_8UC1);
EXPECT_GT(cv::mean(rightOut)[0], cv::mean(right)[0]);
}
TEST(Util2dTest, StereoExposureCompensationNeedsTwo8BitsImagesOfTheSameSize)
{
cv::Mat left(120, 160, CV_8UC1, cv::Scalar(100));
cv::Mat right(60, 80, CV_8UC1, cv::Scalar(50));
cv::Mat leftOut, rightOut;
EXPECT_FALSE(util2d::stereoExposureCompensation(left, right, leftOut, rightOut));
EXPECT_EQ(cv::mean(rightOut)[0], 50.0) << "copied as it is";
cv::Mat left16(120, 160, CV_16UC1, cv::Scalar(1000));
cv::Mat right16(120, 160, CV_16UC1, cv::Scalar(500));
EXPECT_FALSE(util2d::stereoExposureCompensation(left16, right16, leftOut, rightOut));
}
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>rtabmap</name>
<version>0.24.1</version>
<version>0.24.2</version>
<description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>