Stereo exposure compensation (#1795)

* Moved stereo expsorure compensation to its own global function

* bump version

* some stats

CMakeLists.txt

@@ -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

@@ -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

@@ -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

@@ -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);
+			cv::Mat imgLeft;
-			std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
+			cv::Mat imgRight;
-			std::vector<cv::UMat> images;
+			if(util2d::stereoExposureCompensation(data.imageRaw(), data.rightRaw(), imgLeft, imgRight))
-			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));
+				data.setStereoImage(imgLeft, imgRight, data.stereoCameraModels()[0]);
-			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]);
 			if(info) info->timeStereoExposureCompensation = timer.ticks();
-#endif
 		}
 		else
 		{

corelib/src/util2d.cpp

@@ -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

@@ -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,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>