OpenCV_4.2.0/opencv_contrib-4.2.0/modules/structured_light/test/test_plane.cpp

359 lines
12 KiB
C++

/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2015, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "test_precomp.hpp"
namespace opencv_test { namespace {
const string STRUCTURED_LIGHT_DIR = "structured_light";
const string FOLDER_DATA = "data";
/****************************************************************************************\
* Plane test *
\****************************************************************************************/
class CV_PlaneTest : public cvtest::BaseTest
{
public:
CV_PlaneTest();
~CV_PlaneTest();
//////////////////////////////////////////////////////////////////////////////////////////////////
// From rgbd module: since I needed the distance method of plane class, I copied the class from rgb module
// it will be made a pull request to make Plane class public
/** Structure defining a plane. The notations are from the second paper */
class PlaneBase
{
public:
PlaneBase(const Vec3f & m, const Vec3f &n_in, int index) :
index_(index),
n_(n_in),
m_sum_(Vec3f(0, 0, 0)),
m_(m),
Q_(Matx33f::zeros()),
mse_(0),
K_(0)
{
UpdateD();
}
virtual ~PlaneBase()
{
}
/** Compute the distance to the plane. This will be implemented by the children to take into account different
* sensor models
* @param p_j
* @return
*/
virtual
float
distance(const Vec3f& p_j) const = 0;
/** The d coefficient in the plane equation ax+by+cz+d = 0
* @return
*/
inline float d() const
{
return d_;
}
/** The normal to the plane
* @return the normal to the plane
*/
const Vec3f &
n() const
{
return n_;
}
/** Update the different coefficients of the plane, based on the new statistics
*/
void UpdateParameters()
{
if( empty() )
return;
m_ = m_sum_ / K_;
// Compute C
Matx33f C = Q_ - m_sum_ * m_.t();
// Compute n
SVD svd(C);
n_ = Vec3f(svd.vt.at<float>(2, 0), svd.vt.at<float>(2, 1), svd.vt.at<float>(2, 2));
mse_ = svd.w.at<float>(2) / K_;
UpdateD();
}
/** Update the different sum of point and sum of point*point.t()
*/
void UpdateStatistics(const Vec3f & point, const Matx33f & Q_local)
{
m_sum_ += point;
Q_ += Q_local;
++K_;
}
inline size_t empty() const
{
return K_ == 0;
}
inline int K() const
{
return K_;
}
/** The index of the plane */
int index_;
protected:
/** The 4th coefficient in the plane equation ax+by+cz+d = 0 */
float d_;
/** Normal of the plane */
Vec3f n_;
private:
inline void UpdateD()
{
// Hessian form (d = nc . p_plane (centroid here) + p)
//d = -1 * n.dot (xyz_centroid);//d =-axP+byP+czP
d_ = -m_.dot(n_);
}
/** The sum of the points */
Vec3f m_sum_;
/** The mean of the points */
Vec3f m_;
/** The sum of pi * pi^\top */
Matx33f Q_;
/** The different matrices we need to update */
Matx33f C_;
float mse_;
/** the number of points that form the plane */
int K_;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** Basic planar child, with no sensor error model
*/
class Plane : public PlaneBase
{
public:
Plane(const Vec3f & m, const Vec3f &n_in, int index) :
PlaneBase(m, n_in, index)
{
}
/** The computed distance is perfect in that case
* @param p_j the point to compute its distance to
* @return
*/
float distance(const Vec3f& p_j) const
{
return std::abs(float(p_j.dot(n_) + d_));
}
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
protected:
void run( int );
};
CV_PlaneTest::CV_PlaneTest(){}
CV_PlaneTest::~CV_PlaneTest(){}
void CV_PlaneTest::run( int )
{
string folder = cvtest::TS::ptr()->get_data_path() + "/" + STRUCTURED_LIGHT_DIR + "/" + FOLDER_DATA + "/";
structured_light::GrayCodePattern::Params params;
params.width = 1280;
params.height = 800;
// Set up GraycodePattern with params
Ptr<structured_light::GrayCodePattern> graycode = structured_light::GrayCodePattern::create( params );
size_t numberOfPatternImages = graycode->getNumberOfPatternImages();
FileStorage fs( folder + "calibrationParameters.yml", FileStorage::READ );
if( !fs.isOpened() )
{
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
}
FileStorage fs2( folder + "gt_plane.yml", FileStorage::READ );
if( !fs.isOpened() )
{
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
}
// Loading ground truth plane parameters
Vec4f plane_coefficients;
Vec3f m;
fs2["plane_coefficients"] >> plane_coefficients;
fs2["m"] >> m;
// Loading calibration parameters
Mat cam1intrinsics, cam1distCoeffs, cam2intrinsics, cam2distCoeffs, R, T;
fs["cam1_intrinsics"] >> cam1intrinsics;
fs["cam2_intrinsics"] >> cam2intrinsics;
fs["cam1_distorsion"] >> cam1distCoeffs;
fs["cam2_distorsion"] >> cam2distCoeffs;
fs["R"] >> R;
fs["T"] >> T;
// Loading white and black images
vector<Mat> blackImages;
vector<Mat> whiteImages;
blackImages.resize( 2 );
whiteImages.resize( 2 );
whiteImages[0] = imread( folder + "pattern_cam1_im43.jpg", 0 );
whiteImages[1] = imread( folder + "pattern_cam2_im43.jpg", 0 );
blackImages[0] = imread( folder + "pattern_cam1_im44.jpg", 0 );
blackImages[1] = imread( folder + "pattern_cam2_im44.jpg", 0 );
Size imagesSize = whiteImages[0].size();
if( ( !cam1intrinsics.data ) || ( !cam2intrinsics.data ) || ( !cam1distCoeffs.data ) || ( !cam2distCoeffs.data ) || ( !R.data )
|| ( !T.data ) || ( !whiteImages[0].data ) || ( !whiteImages[1].data ) || ( !blackImages[0].data )
|| ( !blackImages[1].data ) )
{
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
}
// Computing stereo rectify parameters
Mat R1, R2, P1, P2, Q;
Rect validRoi[2];
stereoRectify( cam1intrinsics, cam1distCoeffs, cam2intrinsics, cam2distCoeffs, imagesSize, R, T, R1, R2, P1, P2, Q, 0,
-1, imagesSize, &validRoi[0], &validRoi[1] );
Mat map1x, map1y, map2x, map2y;
initUndistortRectifyMap( cam1intrinsics, cam1distCoeffs, R1, P1, imagesSize, CV_32FC1, map1x, map1y );
initUndistortRectifyMap( cam2intrinsics, cam2distCoeffs, R2, P2, imagesSize, CV_32FC1, map2x, map2y );
vector<vector<Mat> > captured_pattern;
captured_pattern.resize( 2 );
captured_pattern[0].resize( numberOfPatternImages );
captured_pattern[1].resize( numberOfPatternImages );
// Loading and rectifying pattern images
for( size_t i = 0; i < numberOfPatternImages; i++ )
{
std::ostringstream name1;
name1 << "pattern_cam1_im" << i + 1 << ".jpg";
captured_pattern[0][i] = imread( folder + name1.str(), 0 );
std::ostringstream name2;
name2 << "pattern_cam2_im" << i + 1 << ".jpg";
captured_pattern[1][i] = imread( folder + name2.str(), 0 );
if( (!captured_pattern[0][i].data) || (!captured_pattern[1][i].data) )
{
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
}
remap( captured_pattern[0][i], captured_pattern[0][i], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
remap( captured_pattern[1][i], captured_pattern[1][i], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
}
// Rectifying white and black images
remap( whiteImages[0], whiteImages[0], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
remap( whiteImages[1], whiteImages[1], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
remap( blackImages[0], blackImages[0], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
remap( blackImages[1], blackImages[1], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
// Setting up threshold parameters to reconstruct only the plane in foreground
graycode->setBlackThreshold( 55 );
graycode->setWhiteThreshold( 10 );
// Computing the disparity map
Mat disparityMap;
bool decoded = graycode->decode( captured_pattern, disparityMap, blackImages, whiteImages,
structured_light::DECODE_3D_UNDERWORLD );
EXPECT_TRUE( decoded );
// Computing the point cloud
Mat pointcloud;
disparityMap.convertTo( disparityMap, CV_32FC1 );
reprojectImageTo3D( disparityMap, pointcloud, Q, true, -1 );
// from mm (unit of calibration) to m
pointcloud = pointcloud / 1000;
// Setting up plane with ground truth plane values
Vec3f normal( plane_coefficients.val[0], plane_coefficients.val[1], plane_coefficients.val[2] );
Ptr<PlaneBase> plane = Ptr<PlaneBase>( new Plane( m, normal, 0 ) );
// Computing the distance of every point of the pointcloud from ground truth plane
float sum_d = 0;
int cont = 0;
for( int i = 0; i < disparityMap.rows; i++ )
{
for( int j = 0; j < disparityMap.cols; j++ )
{
float value = disparityMap.at<float>( i, j );
if( value != 0 )
{
Vec3f point = pointcloud.at<Vec3f>( i, j );
sum_d += plane->distance( point );
cont++;
}
}
}
sum_d /= cont;
// test pass if the mean of points distance from ground truth plane is lower than 3 mm
EXPECT_LE( sum_d, 0.003 );
}
/****************************************************************************************\
* Test registration *
\****************************************************************************************/
TEST( GrayCodePattern, plane_reconstruction )
{
CV_PlaneTest test;
test.safe_run();
}
}} // namespace