/** Constructor from a 4x4 homogeneous transformation matrix.
*/
CPose3DQuat::CPose3DQuat(const CMatrixDouble44 &M) : m_quat(UNINITIALIZED_QUATERNION)
{
m_coords[0] = M.get_unsafe(0,3);
m_coords[1] = M.get_unsafe(1,3);
m_coords[2] = M.get_unsafe(2,3);
CPose3D p(M);
p.getAsQuaternion(m_quat);
}
/** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
* \sa getInverseHomogeneousMatrix
*/
void CPose3DQuat::getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const
{
m_quat.rotationMatrixNoResize(out_HM);
out_HM.get_unsafe(0,3) = m_coords[0];
out_HM.get_unsafe(1,3) = m_coords[1];
out_HM.get_unsafe(2,3) = m_coords[2];
out_HM.get_unsafe(3,0) = out_HM.get_unsafe(3,1) = out_HM.get_unsafe(3,2) = 0;
out_HM.get_unsafe(3,3) = 1;
}
/*---------------------------------------------------------------
Returns the corresponding 4x4 homogeneous
transformation matrix for the point(translation),
or pose (translation+orientation).
---------------------------------------------------------------*/
void CPose2D::getHomogeneousMatrix(CMatrixDouble44& m) const
{
m.unit(4,1.0);
m.set_unsafe(0,3, m_coords[0] );
m.set_unsafe(1,3, m_coords[1] );
update_cached_cos_sin();
m.get_unsafe(0,0) = m_cosphi; m.get_unsafe(0,1)=-m_sinphi;
m.get_unsafe(1,0) = m_sinphi; m.get_unsafe(1,1)= m_cosphi;
}
/*---------------------------------------------------------------
HornMethod
---------------------------------------------------------------*/
double scanmatching::HornMethod(
const vector_double &inVector,
vector_double &outVector, // The output vector
bool forceScaleToUnity )
{
MRPT_START
vector_double input;
input.resize( inVector.size() );
input = inVector;
outVector.resize( 7 );
// Compute the centroids
TPoint3D cL(0,0,0), cR(0,0,0);
const size_t nMatches = input.size()/6;
ASSERT_EQUAL_(input.size()%6, 0)
for( unsigned int i = 0; i < nMatches; i++ )
{
cL.x += input[i*6+3];
cL.y += input[i*6+4];
cL.z += input[i*6+5];
cR.x += input[i*6+0];
cR.y += input[i*6+1];
cR.z += input[i*6+2];
}
ASSERT_ABOVE_(nMatches,0)
const double F = 1.0/nMatches;
cL *= F;
cR *= F;
CMatrixDouble33 S; // S.zeros(); // Zeroed by default
// Substract the centroid and compute the S matrix of cross products
for( unsigned int i = 0; i < nMatches; i++ )
{
input[i*6+3] -= cL.x;
input[i*6+4] -= cL.y;
input[i*6+5] -= cL.z;
input[i*6+0] -= cR.x;
input[i*6+1] -= cR.y;
input[i*6+2] -= cR.z;
S.get_unsafe(0,0) += input[i*6+3]*input[i*6+0];
S.get_unsafe(0,1) += input[i*6+3]*input[i*6+1];
S.get_unsafe(0,2) += input[i*6+3]*input[i*6+2];
S.get_unsafe(1,0) += input[i*6+4]*input[i*6+0];
S.get_unsafe(1,1) += input[i*6+4]*input[i*6+1];
S.get_unsafe(1,2) += input[i*6+4]*input[i*6+2];
S.get_unsafe(2,0) += input[i*6+5]*input[i*6+0];
S.get_unsafe(2,1) += input[i*6+5]*input[i*6+1];
S.get_unsafe(2,2) += input[i*6+5]*input[i*6+2];
}
// Construct the N matrix
CMatrixDouble44 N; // N.zeros(); // Zeroed by default
N.set_unsafe( 0,0,S.get_unsafe(0,0) + S.get_unsafe(1,1) + S.get_unsafe(2,2) );
N.set_unsafe( 0,1,S.get_unsafe(1,2) - S.get_unsafe(2,1) );
N.set_unsafe( 0,2,S.get_unsafe(2,0) - S.get_unsafe(0,2) );
N.set_unsafe( 0,3,S.get_unsafe(0,1) - S.get_unsafe(1,0) );
N.set_unsafe( 1,0,N.get_unsafe(0,1) );
N.set_unsafe( 1,1,S.get_unsafe(0,0) - S.get_unsafe(1,1) - S.get_unsafe(2,2) );
N.set_unsafe( 1,2,S.get_unsafe(0,1) + S.get_unsafe(1,0) );
N.set_unsafe( 1,3,S.get_unsafe(2,0) + S.get_unsafe(0,2) );
N.set_unsafe( 2,0,N.get_unsafe(0,2) );
N.set_unsafe( 2,1,N.get_unsafe(1,2) );
N.set_unsafe( 2,2,-S.get_unsafe(0,0) + S.get_unsafe(1,1) - S.get_unsafe(2,2) );
N.set_unsafe( 2,3,S.get_unsafe(1,2) + S.get_unsafe(2,1) );
N.set_unsafe( 3,0,N.get_unsafe(0,3) );
N.set_unsafe( 3,1,N.get_unsafe(1,3) );
N.set_unsafe( 3,2,N.get_unsafe(2,3) );
N.set_unsafe( 3,3,-S.get_unsafe(0,0) - S.get_unsafe(1,1) + S.get_unsafe(2,2) );
// q is the quaternion correspondent to the greatest eigenvector of the N matrix (last column in Z)
CMatrixDouble44 Z, D;
vector_double v;
N.eigenVectors( Z, D );
Z.extractCol( Z.getColCount()-1, v );
ASSERTDEB_( fabs( sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3] ) - 1.0 ) < 0.1 );
// Make q_r > 0
if( v[0] < 0 ){ v[0] *= -1; v[1] *= -1; v[2] *= -1; v[3] *= -1; }
CPose3DQuat q; // Create a pose rotation with the quaternion
for(unsigned int i = 0; i < 4; i++ ) // insert the quaternion part
outVector[i+3] = q[i+3] = v[i];
// Compute scale
double num = 0.0;
double den = 0.0;
for( unsigned int i = 0; i < nMatches; i++ )
{
num += square( input[i*6+0] ) + square( input[i*6+1] ) + square( input[i*6+2] );
den += square( input[i*6+3] ) + square( input[i*6+4] ) + square( input[i*6+5] );
} // end-for
// The scale:
double s = std::sqrt( num/den );
// Enforce scale to be 1
if( forceScaleToUnity )
s = 1.0;
TPoint3D pp;
q.composePoint( cL.x, cL.y, cL.z, pp.x, pp.y, pp.z );
pp*=s;
outVector[0] = cR.x - pp.x; // X
outVector[1] = cR.y - pp.y; // Y
outVector[2] = cR.z - pp.z; // Z
return s; // return scale
MRPT_END
}
void CDifodoDatasets::loadConfiguration(unsigned int &i_rows, unsigned int &i_cols,
vector<CPose3D> &v_poses,
const string &rawlogFileName,
vector<unsigned int> &cameras_order,
bool visualizeResults )
{
visualize_results = visualizeResults;
fovh = M_PI*62.5/180.0; //Larger FOV because depth is registered with color
fovv = M_PI*48.5/180.0;
rows = i_rows;
cols = i_cols;
cam_mode = 2;
fast_pyramid = false;
downsample = 1; // 1 - 640x480, 2 - 320x240, 4 - 160x120
ctf_levels = 5;
fast_pyramid = true;
// Load cameras' extrinsic calibrations
//==================================================================
cams_oder = cameras_order;
for ( int c = 0; c < NC; c++ )
{
cam_pose[c] = v_poses[c];
CMatrixDouble44 homoMatrix;
cam_pose[c].getHomogeneousMatrix(homoMatrix);
calib_mat[c] = (CMatrixFloat44)homoMatrix.inverse();
}
// Open Rawlog File
//==================================================================
if (!dataset.loadFromRawLogFile(rawlogFileName))
throw std::runtime_error("\nCouldn't open rawlog dataset file for input...");
rawlog_count = 0;
// Set external images directory:
const string imgsPath = CRawlog::detectImagesDirectory(rawlogFileName);
CImage::IMAGES_PATH_BASE = imgsPath;
// Resize matrices and adjust parameters
//=========================================================
width = 640/(cam_mode*downsample);
height = 480/(cam_mode*downsample);
repr_level = utils::round(log(float(width/cols))/log(2.f));
//Resize pyramid
const unsigned int pyr_levels = round(log(float(width/cols))/log(2.f)) + ctf_levels;
for (unsigned int c=0; c<NC; c++)
{
for (unsigned int i = 0; i<pyr_levels; i++)
{
unsigned int s = pow(2.f,int(i));
cols_i = width/s; rows_i = height/s;
depth[c][i].resize(rows_i, cols_i);
depth_inter[c][i].resize(rows_i, cols_i);
depth_old[c][i].resize(rows_i, cols_i);
depth[c][i].assign(0.0f);
depth_old[c][i].assign(0.0f);
xx[c][i].resize(rows_i, cols_i);
xx_inter[c][i].resize(rows_i, cols_i);
xx_old[c][i].resize(rows_i, cols_i);
xx[c][i].assign(0.0f);
xx_old[c][i].assign(0.0f);
yy[c][i].resize(rows_i, cols_i);
yy_inter[c][i].resize(rows_i, cols_i);
yy_old[c][i].resize(rows_i, cols_i);
yy[c][i].assign(0.0f);
yy_old[c][i].assign(0.0f);
zz_global[c][i].resize(rows_i, cols_i);
xx_global[c][i].resize(rows_i, cols_i);
yy_global[c][i].resize(rows_i, cols_i);
if (cols_i <= cols)
{
depth_warped[c][i].resize(rows_i,cols_i);
xx_warped[c][i].resize(rows_i,cols_i);
yy_warped[c][i].resize(rows_i,cols_i);
}
}
//Resize matrix that store the original depth image
depth_wf[c].setSize(height,width);
}
//Resize the transformation matrices
for (unsigned int l = 0; l<pyr_levels; l++)
global_trans[l].resize(4,4);
for (unsigned int c=0; c<NC; c++)
for (unsigned int l = 0; l<pyr_levels; l++)
transformations[c][l].resize(4,4);
}
/*---------------------------------------------------------------
se3_l2 (old "HornMethod()")
---------------------------------------------------------------*/
bool se3_l2_internal(
std::vector<mrpt::math::TPoint3D> & points_this, // IN/OUT: It gets modified!
std::vector<mrpt::math::TPoint3D> & points_other, // IN/OUT: It gets modified!
mrpt::poses::CPose3DQuat & out_transform,
double & out_scale,
bool forceScaleToUnity)
{
MRPT_START
ASSERT_EQUAL_(points_this.size(),points_other.size())
// Compute the centroids
TPoint3D ct_others(0,0,0), ct_this(0,0,0);
const size_t nMatches = points_this.size();
if (nMatches<3)
return false; // Nothing we can estimate without 3 points!!
for(size_t i = 0; i < nMatches; i++ )
{
ct_others += points_other[i];
ct_this += points_this [i];
}
const double F = 1.0/nMatches;
ct_others *= F;
ct_this *= F;
CMatrixDouble33 S; // Zeroed by default
// Substract the centroid and compute the S matrix of cross products
for(size_t i = 0; i < nMatches; i++ )
{
points_this[i] -= ct_this;
points_other[i] -= ct_others;
S.get_unsafe(0,0) += points_other[i].x * points_this[i].x;
S.get_unsafe(0,1) += points_other[i].x * points_this[i].y;
S.get_unsafe(0,2) += points_other[i].x * points_this[i].z;
S.get_unsafe(1,0) += points_other[i].y * points_this[i].x;
S.get_unsafe(1,1) += points_other[i].y * points_this[i].y;
S.get_unsafe(1,2) += points_other[i].y * points_this[i].z;
S.get_unsafe(2,0) += points_other[i].z * points_this[i].x;
S.get_unsafe(2,1) += points_other[i].z * points_this[i].y;
S.get_unsafe(2,2) += points_other[i].z * points_this[i].z;
}
// Construct the N matrix
CMatrixDouble44 N; // Zeroed by default
N.set_unsafe( 0,0,S.get_unsafe(0,0) + S.get_unsafe(1,1) + S.get_unsafe(2,2) );
N.set_unsafe( 0,1,S.get_unsafe(1,2) - S.get_unsafe(2,1) );
N.set_unsafe( 0,2,S.get_unsafe(2,0) - S.get_unsafe(0,2) );
N.set_unsafe( 0,3,S.get_unsafe(0,1) - S.get_unsafe(1,0) );
N.set_unsafe( 1,0,N.get_unsafe(0,1) );
N.set_unsafe( 1,1,S.get_unsafe(0,0) - S.get_unsafe(1,1) - S.get_unsafe(2,2) );
N.set_unsafe( 1,2,S.get_unsafe(0,1) + S.get_unsafe(1,0) );
N.set_unsafe( 1,3,S.get_unsafe(2,0) + S.get_unsafe(0,2) );
N.set_unsafe( 2,0,N.get_unsafe(0,2) );
N.set_unsafe( 2,1,N.get_unsafe(1,2) );
N.set_unsafe( 2,2,-S.get_unsafe(0,0) + S.get_unsafe(1,1) - S.get_unsafe(2,2) );
N.set_unsafe( 2,3,S.get_unsafe(1,2) + S.get_unsafe(2,1) );
N.set_unsafe( 3,0,N.get_unsafe(0,3) );
N.set_unsafe( 3,1,N.get_unsafe(1,3) );
N.set_unsafe( 3,2,N.get_unsafe(2,3) );
N.set_unsafe( 3,3,-S.get_unsafe(0,0) - S.get_unsafe(1,1) + S.get_unsafe(2,2) );
// q is the quaternion correspondent to the greatest eigenvector of the N matrix (last column in Z)
CMatrixDouble44 Z, D;
vector<double> v;
N.eigenVectors( Z, D );
Z.extractCol( Z.getColCount()-1, v );
ASSERTDEB_( fabs( sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3] ) - 1.0 ) < 0.1 );
// Make q_r > 0
if( v[0] < 0 ) {
v[0] *= -1;
v[1] *= -1;
v[2] *= -1;
v[3] *= -1;
}
// out_transform: Create a pose rotation with the quaternion
for(unsigned int i = 0; i < 4; i++ ) // insert the quaternion part
out_transform[i+3] = v[i];
// Compute scale
double s;
if( forceScaleToUnity )
{
s = 1.0; // Enforce scale to be 1
}
else
{
double num = 0.0;
double den = 0.0;
for(size_t i = 0; i < nMatches; i++ )
{
num += square( points_other[i].x ) + square( points_other[i].y ) + square( points_other[i].z );
den += square( points_this[i].x ) + square( points_this[i].y ) + square( points_this[i].z );
} // end-for
// The scale:
s = std::sqrt( num/den );
}
TPoint3D pp;
out_transform.composePoint( ct_others.x, ct_others.y, ct_others.z, pp.x, pp.y, pp.z );
pp*=s;
out_transform[0] = ct_this.x - pp.x; // X
out_transform[1] = ct_this.y - pp.y; // Y
out_transform[2] = ct_this.z - pp.z; // Z
out_scale=s; // return scale
return true;
MRPT_END
} // end se3_l2_internal()
/* -------------------------------------------------------
checkerBoardCameraCalibration
------------------------------------------------------- */
bool mrpt::vision::checkerBoardCameraCalibration(
TCalibrationImageList &images,
unsigned int check_size_x,
unsigned int check_size_y,
double check_squares_length_X_meters,
double check_squares_length_Y_meters,
mrpt::utils::TCamera &out_camera_params,
bool normalize_image,
double *out_MSE,
bool skipDrawDetectedImgs,
bool useScaramuzzaAlternativeDetector
)
{
MRPT_UNUSED_PARAM(skipDrawDetectedImgs);
#if MRPT_HAS_OPENCV
try
{
ASSERT_(check_size_x>2)
ASSERT_(check_size_y>2)
ASSERT_(check_squares_length_X_meters>0)
ASSERT_(check_squares_length_Y_meters>0)
if (images.size()<1)
{
std::cout << "ERROR: No input images." << std::endl;
return false;
}
const unsigned CORNERS_COUNT = check_size_x * check_size_y;
const CvSize check_size = cvSize(check_size_x, check_size_y);
// Fill the pattern of expected pattern points only once out of the loop:
vector<cv::Point3f> pattern_obj_points(CORNERS_COUNT);
{
unsigned int y,k;
for( y = 0, k = 0; y < check_size_y; y++ )
{
for( unsigned int x = 0; x < check_size_x; x++, k++ )
{
pattern_obj_points[k].x =-check_squares_length_X_meters * x; // The "-" is for convenience, so the camera poses appear with Z>0
pattern_obj_points[k].y = check_squares_length_Y_meters * y;
pattern_obj_points[k].z = 0;
}
}
}
// First: Assure all images are loaded:
// -------------------------------------------
TCalibrationImageList::iterator it;
for (it=images.begin();it!=images.end();++it)
{
TImageCalibData &dat = it->second;
dat.projectedPoints_distorted.clear(); // Clear reprojected points.
dat.projectedPoints_undistorted.clear();
// Skip if images are marked as "externalStorage":
if (!dat.img_original.isExternallyStored() && !mrpt::system::extractFileExtension(it->first).empty() )
{
if (!dat.img_original.loadFromFile(it->first))
THROW_EXCEPTION_CUSTOM_MSG1("Error reading image: %s",it->first.c_str());
dat.img_checkboard = dat.img_original;
dat.img_rectified = dat.img_original;
}
}
// For each image, find checkerboard corners:
// -----------------------------------------------
vector<vector<cv::Point3f> > objectPoints; // final container for detected stuff
vector<vector<cv::Point2f> > imagePoints; // final container for detected stuff
unsigned int valid_detected_imgs = 0;
vector<string> pointsIdx2imageFile;
cv::Size imgSize(0,0);
unsigned int i;
for (i=0,it=images.begin();it!=images.end();it++,i++)
{
TImageCalibData &dat = it->second;
// Make grayscale version:
const CImage img_gray( dat.img_original, FAST_REF_OR_CONVERT_TO_GRAY );
if (!i)
{
imgSize = cv::Size(img_gray.getWidth(),img_gray.getHeight() );
out_camera_params.ncols = imgSize.width;
out_camera_params.nrows = imgSize.height;
}
else
{
if (imgSize.height != (int)img_gray.getHeight() || imgSize.width != (int)img_gray.getWidth())
{
std::cout << "ERROR: All the images must have the same size" << std::endl;
return false;
}
}
// Try with expanded versions of the image if it fails to detect the checkerboard:
unsigned corners_count;
bool corners_found=false;
corners_count = CORNERS_COUNT;
vector<cv::Point2f> this_img_pts(CORNERS_COUNT); // Temporary buffer for points, to be added if the points pass the checks.
dat.detected_corners.clear();
// Do detection (this includes the "refine corners" with cvFindCornerSubPix):
vector<TPixelCoordf> detectedCoords;
corners_found = mrpt::vision::findChessboardCorners(
img_gray,
detectedCoords,
check_size_x,check_size_y,
normalize_image, // normalize_image
useScaramuzzaAlternativeDetector
);
corners_count = detectedCoords.size();
// Copy the data into the overall array of coords:
ASSERT_(detectedCoords.size()<=CORNERS_COUNT);
for (size_t p=0;p<detectedCoords.size();p++)
{
this_img_pts[p].x = detectedCoords[p].x;
this_img_pts[p].y = detectedCoords[p].y;
}
if (corners_found && corners_count!=CORNERS_COUNT)
corners_found = false;
cout << format("Img %s: %s\n", mrpt::system::extractFileName(it->first).c_str() , corners_found ? "DETECTED" : "NOT DETECTED" );
if( corners_found )
{
// save the corners in the data structure:
int x, y;
unsigned int k;
for( y = 0, k = 0; y < check_size.height; y++ )
for( x = 0; x < check_size.width; x++, k++ )
dat.detected_corners.push_back( mrpt::utils::TPixelCoordf( this_img_pts[k].x, this_img_pts[k].y ) );
// Draw the checkerboard in the corresponding image:
// ----------------------------------------------------
if ( !dat.img_original.isExternallyStored() )
{
const int r = 4;
CvPoint prev_pt= cvPoint(0, 0);
const int line_max = 8;
CvScalar line_colors[8];
line_colors[0] = CV_RGB(255,0,0);
line_colors[1] = CV_RGB(255,128,0);
line_colors[2] = CV_RGB(255,128,0);
line_colors[3] = CV_RGB(200,200,0);
line_colors[4] = CV_RGB(0,255,0);
line_colors[5] = CV_RGB(0,200,200);
line_colors[6] = CV_RGB(0,0,255);
line_colors[7] = CV_RGB(255,0,255);
// Checkboad as color image:
dat.img_original.colorImage( dat.img_checkboard );
void *rgb_img = dat.img_checkboard.getAs<IplImage>();
for( y = 0, k = 0; y < check_size.height; y++ )
{
CvScalar color = line_colors[y % line_max];
for( x = 0; x < check_size.width; x++, k++ )
{
CvPoint pt;
pt.x = cvRound(this_img_pts[k].x);
pt.y = cvRound(this_img_pts[k].y);
if( k != 0 ) cvLine( rgb_img, prev_pt, pt, color );
cvLine( rgb_img,
cvPoint( pt.x - r, pt.y - r ),
cvPoint( pt.x + r, pt.y + r ), color );
cvLine( rgb_img,
cvPoint( pt.x - r, pt.y + r),
cvPoint( pt.x + r, pt.y - r), color );
cvCircle( rgb_img, pt, r+1, color );
prev_pt = pt;
}
}
}
// Accept this image as good:
pointsIdx2imageFile.push_back( it->first );
imagePoints.push_back( this_img_pts );
objectPoints.push_back( pattern_obj_points );
valid_detected_imgs++;
}
} // end find corners
std::cout << valid_detected_imgs << " valid images." << std::endl;
if (!valid_detected_imgs)
{
std::cout << "ERROR: No valid images. Perhaps the checkerboard size is incorrect?" << std::endl;
return false;
}
// ---------------------------------------------
// Calculate the camera parameters
// ---------------------------------------------
// Calibrate camera
cv::Mat cameraMatrix, distCoeffs(1,5,CV_64F,cv::Scalar::all(0));
vector<cv::Mat> rvecs, tvecs;
const double cv_calib_err =
cv::calibrateCamera(
objectPoints,imagePoints,imgSize,
cameraMatrix, distCoeffs, rvecs, tvecs,
0 /*flags*/ );
// Load matrix:
out_camera_params.intrinsicParams = CMatrixDouble33( cameraMatrix.ptr<double>() );
out_camera_params.dist.assign(0);
for (int i=0;i<5;i++)
out_camera_params.dist[i] = distCoeffs.ptr<double>()[i];
// Load camera poses:
for (i=0;i<valid_detected_imgs;i++)
{
CMatrixDouble44 HM;
HM.zeros();
HM(3,3)=1;
{
// Convert rotation vectors -> rot matrices:
cv::Mat cv_rot;
cv::Rodrigues(rvecs[i],cv_rot);
Eigen::Matrix3d rot;
cv::my_cv2eigen(cv_rot, rot );
HM.block<3,3>(0,0) = rot;
}
{
Eigen::Matrix<double,3,1> trans;
cv::my_cv2eigen(tvecs[i], trans );
HM.block<3,1>(0,3) = trans;
}
CPose3D p = CPose3D(0,0,0) - CPose3D(HM);
images[ pointsIdx2imageFile[i] ].reconstructed_camera_pose = p;
std::cout << "Img: " << mrpt::system::extractFileName(pointsIdx2imageFile[i]) << ": " << p << std::endl;
}
{
CConfigFileMemory cfg;
out_camera_params.saveToConfigFile("CAMERA_PARAMS",cfg);
std::cout << cfg.getContent() << std::endl;
}
// ----------------------------------------
// Undistort images:
// ----------------------------------------
for (it=images.begin();it!=images.end();++it)
{
TImageCalibData &dat = it->second;
if (!dat.img_original.isExternallyStored())
dat.img_original.rectifyImage( dat.img_rectified, out_camera_params);
} // end undistort
// -----------------------------------------------
// Reproject points to measure the fit sqr error
// -----------------------------------------------
double sqrErr = 0;
for (i=0;i<valid_detected_imgs;i++)
{
TImageCalibData & dat = images[ pointsIdx2imageFile[i] ];
if (dat.detected_corners.size()!=CORNERS_COUNT) continue;
// Reproject all the points into pixel coordinates:
// -----------------------------------------------------
vector<TPoint3D> lstPatternPoints(CORNERS_COUNT); // Points as seen from the camera:
for (unsigned int p=0;p<CORNERS_COUNT;p++)
lstPatternPoints[p] = TPoint3D(pattern_obj_points[p].x,pattern_obj_points[p].y,pattern_obj_points[p].z);
vector<TPixelCoordf> &projectedPoints = dat.projectedPoints_undistorted;
vector<TPixelCoordf> &projectedPoints_distorted = dat.projectedPoints_distorted;
vision::pinhole::projectPoints_no_distortion(
lstPatternPoints, // Input points
dat.reconstructed_camera_pose,
out_camera_params.intrinsicParams, // calib matrix
projectedPoints // Output points in pixels
);
vision::pinhole::projectPoints_with_distortion(
lstPatternPoints, // Input points
dat.reconstructed_camera_pose,
out_camera_params.intrinsicParams, // calib matrix
out_camera_params.getDistortionParamsAsVector(),
projectedPoints_distorted// Output points in pixels
);
ASSERT_(projectedPoints.size()==CORNERS_COUNT);
ASSERT_(projectedPoints_distorted.size()==CORNERS_COUNT);
for (unsigned int p=0;p<CORNERS_COUNT;p++)
{
const double px = projectedPoints[p].x;
const double py = projectedPoints[p].y;
const double px_d = projectedPoints_distorted[p].x;
const double py_d = projectedPoints_distorted[p].y;
// Only draw if the img is NOT external:
if (!dat.img_original.isExternallyStored())
{
if( px >= 0 && px < imgSize.width && py >= 0 && py < imgSize.height )
cvCircle( dat.img_rectified.getAs<IplImage>(), cvPoint(px,py), 4, CV_RGB(0,0,255) );
}
// Accumulate error:
sqrErr+=square(px_d-dat.detected_corners[p].x)+square(py_d-dat.detected_corners[p].y); // Error relative to the original (distorted) image.
}
}
if (valid_detected_imgs)
{
sqrErr /= CORNERS_COUNT*valid_detected_imgs;
std::cout << "Average err. of reprojection: " << sqrt(sqrErr) << " pixels (OpenCV error=" << cv_calib_err << ")\n";
}
if(out_MSE) *out_MSE = sqrt(sqrErr);
return true;
}
catch(std::exception &e)
{
std::cout << e.what() << std::endl;
return false;
}
#else
THROW_EXCEPTION("Function not available: MRPT was compiled without OpenCV")
#endif
}
