diff --git a/CMakeLists.txt b/CMakeLists.txt index 177f8448..e79dec79 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/corelib/include/rtabmap/core/util2d.h b/corelib/include/rtabmap/core/util2d.h index da0cfe6b..e3b27867 100644 --- a/corelib/include/rtabmap/core/util2d.h +++ b/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 & 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 * gains = 0); + /** * @brief Converts a color from HSV (Hue, Saturation, Value) to RGB. * diff --git a/corelib/src/OdometryInfo.cpp b/corelib/src/OdometryInfo.cpp index 4b1d3c90..cab2eaea 100644 --- a/corelib/src/OdometryInfo.cpp +++ b/corelib/src/OdometryInfo.cpp @@ -187,6 +187,12 @@ std::map 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()) diff --git a/corelib/src/SensorCaptureThread.cpp b/corelib/src/SensorCaptureThread.cpp index ab284816..37951861 100644 --- a/corelib/src/SensorCaptureThread.cpp +++ b/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 #include #include #include @@ -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 compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN); - std::vector topLeftCorners(2, cv::Point(0,0)); - std::vector images; - std::vector 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 { diff --git a/corelib/src/util2d.cpp b/corelib/src/util2d.cpp index 584dd342..271ca747 100644 --- a/corelib/src/util2d.cpp +++ b/corelib/src/util2d.cpp @@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #if CV_MAJOR_VERSION >= 3 #include +#include #endif #if CV_MAJOR_VERSION < 5 @@ -2139,6 +2140,66 @@ cv::Mat exposureFusion(const std::vector & images) return fusion; } +bool stereoExposureCompensation( + const cv::Mat & left, + const cv::Mat & right, + cv::Mat & leftOut, + cv::Mat & rightOut, + std::vector * 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 compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN); + std::vector topLeftCorners(2, cv::Point(0,0)); + std::vector images; + std::vector 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; diff --git a/corelib/test/test_util2d.cpp b/corelib/test/test_util2d.cpp index deb0bd29..ca7e3b08 100644 --- a/corelib/test/test_util2d.cpp +++ b/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); -} \ No newline at end of file +} +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 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)); +} diff --git a/package.xml b/package.xml index ade42f62..40a9da07 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.24.1 + 0.24.2 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe