static void calcKeyPointProjections( const vector<KeyPoint>& src, const Mat_<double>& H, vector<KeyPoint>& dst )
{
if( !src.empty() )
{
CV_Assert( !H.empty() && H.cols == 3 && H.rows == 3);
dst.resize(src.size());
vector<KeyPoint>::const_iterator srcIt = src.begin();
vector<KeyPoint>::iterator dstIt = dst.begin();
for( ; srcIt != src.end(); ++srcIt, ++dstIt )
{
Point2f dstPt = applyHomography(H, srcIt->pt);
float srcSize2 = srcIt->size * srcIt->size;
Mat_<double> M(2, 2);
M(0,0) = M(1,1) = 1./srcSize2;
M(1,0) = M(0,1) = 0;
Mat_<double> invM; invert(M, invM);
Mat_<double> Aff; linearizeHomographyAt(H, srcIt->pt, Aff);
Mat_<double> dstM; invert(Aff*invM*Aff.t(), dstM);
Mat_<double> eval; eigen( dstM, eval );
CV_Assert( eval(0,0) && eval(1,0) );
float dstSize = (float)pow(1./(eval(0,0)*eval(1,0)), 0.25);
// TODO: check angle projection
float srcAngleRad = (float)(srcIt->angle*CV_PI/180);
Point2f vec1(cos(srcAngleRad), sin(srcAngleRad)), vec2;
vec2.x = (float)(Aff(0,0)*vec1.x + Aff(0,1)*vec1.y);
vec2.y = (float)(Aff(1,0)*vec1.x + Aff(0,1)*vec1.y);
float dstAngleGrad = fastAtan2(vec2.y, vec2.x);
*dstIt = KeyPoint( dstPt, dstSize, dstAngleGrad, srcIt->response, srcIt->octave, srcIt->class_id );
}
}
}
void FaceAnalyser::UpdatePredictionTrack(Mat_<unsigned int>& prediction_corr_histogram, int& prediction_correction_count, vector<double>& correction, const vector<pair<string, double>>& predictions, double ratio, int num_bins, double min_val, double max_val, int min_frames)
{
double length = max_val - min_val;
if(length < 0)
length = -length;
correction.resize(predictions.size(), 0);
// The median update
if(prediction_corr_histogram.empty())
{
prediction_corr_histogram = Mat_<unsigned int>(predictions.size(), num_bins, (unsigned int)0);
}
for(int i = 0; i < prediction_corr_histogram.rows; ++i)
{
// Find the bins corresponding to the current descriptor
int index = (predictions[i].second - min_val)*((double)num_bins)/(length);
if(index < 0)
{
index = 0;
}
else if(index > num_bins - 1)
{
index = num_bins - 1;
}
prediction_corr_histogram.at<unsigned int>(i, index)++;
}
// Update the histogram count
prediction_correction_count++;
if(prediction_correction_count >= min_frames)
{
// Recompute the correction
int cutoff_point = ratio * prediction_correction_count;
// For each dimension
for(int i = 0; i < prediction_corr_histogram.rows; ++i)
{
int cummulative_sum = 0;
for(int j = 0; j < prediction_corr_histogram.cols; ++j)
{
cummulative_sum += prediction_corr_histogram.at<unsigned int>(i, j);
if(cummulative_sum > cutoff_point)
{
double corr = min_val + j * (length/num_bins);
correction[i] = corr;
break;
}
}
}
}
}
void EllipticKeyPoint::calcProjection( const vector<EllipticKeyPoint>& src, const Mat_<double>& H, vector<EllipticKeyPoint>& dst )
{
if( !src.empty() )
{
assert( !H.empty() && H.cols == 3 && H.rows == 3);
dst.resize(src.size());
vector<EllipticKeyPoint>::const_iterator srcIt = src.begin();
vector<EllipticKeyPoint>::iterator dstIt = dst.begin();
for( ; srcIt != src.end(); ++srcIt, ++dstIt )
srcIt->calcProjection(H, *dstIt);
}
}
int copyMakeBorder(/*const*/ Mat_<_Tp, chs>& src, Mat_<_Tp, chs>& dst, int top, int bottom, int left, int right, int borderType, const Scalar& value = Scalar())
{
FBC_Assert(top >= 0 && bottom >= 0 && left >= 0 && right >= 0);
if (src.isSubmatrix() && (borderType & BORDER_ISOLATED) == 0) {
Size wholeSize;
Point ofs;
src.locateROI(wholeSize, ofs);
int dtop = std::min(ofs.y, top);
int dbottom = std::min(wholeSize.height - src.rows - ofs.y, bottom);
int dleft = std::min(ofs.x, left);
int dright = std::min(wholeSize.width - src.cols - ofs.x, right);
src.adjustROI(dtop, dbottom, dleft, dright);
top -= dtop;
left -= dleft;
bottom -= dbottom;
right -= dright;
}
if (dst.empty() || dst.rows != (src.rows + top + bottom) || dst.cols != (src.cols + left + right)) {
dst.release();
dst = Mat_<_Tp, chs>(src.rows + top + bottom, src.cols + left + right);
}
if (top == 0 && left == 0 && bottom == 0 && right == 0) {
if (src.data != dst.data || src.step != dst.step)
src.copyTo(dst);
return 0;
}
borderType &= ~BORDER_ISOLATED;
if (borderType != BORDER_CONSTANT) {
copyMakeBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, src.elemSize(), borderType);
} else {
int cn = src.channels, cn1 = cn;
AutoBuffer<double> buf(cn);
scalarToRawData<_Tp, chs>(value, buf, cn);
copyMakeConstBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, (int)src.elemSize(), (uchar*)(double*)buf);
}
return 0;
}
int main (int argc, char **argv)
{
vector<string> arguments = get_arguments(argc, argv);
// Some initial parameters that can be overriden from command line
vector<string> files, dDirs, outposes, outvideos, outfeatures;
// By default try webcam
int device = 0;
// cx and cy aren't always half dimx or half dimy, so need to be able to override it (start with unit vals and init them if none specified)
float fx = 500, fy = 500, cx = 0, cy = 0;
int dimx = 0, dimy = 0;
bool useCLMTracker = true;
CLMWrapper::CLMParameters clmParams(arguments);
clmParams.wSizeCurrent = clmParams.wSizeInit;
PoseDetectorHaar::PoseDetectorHaarParameters haarParams;
#if OS_UNIX
haarParams.ClassifierLocation = "classifiers/haarcascade_frontalface_alt.xml";
#else
haarParams.ClassifierLocation = "classifiers/haarcascade_frontalface_alt.xml";
#endif
// Get the input output file parameters
CLMWrapper::get_video_input_output_params(files, dDirs, outposes, outvideos, outfeatures, arguments);
// Get camera parameters
CLMWrapper::get_camera_params(fx, fy, cx, cy, dimx, dimy, arguments);
// The modules that are being used for tracking
CLMTracker::TrackerCLM clmModel;
// Face detector initialisation
CascadeClassifier classifier(haarParams.ClassifierLocation);
if(classifier.empty())
{
string err = "Could not open a face detector at: " + haarParams.ClassifierLocation;
FATAL_STREAM( err );
}
bool done = false;
int f_n = -1;
while(!done)
{
string file;
// We might specify multiple video files as arguments
if(files.size() > 0)
{
f_n++;
file = files[f_n];
}
bool readDepth = !dDirs.empty();
// Do some grabbing
VideoCapture vCap;
if( file.size() > 0 )
{
INFO_STREAM( "Attempting to read from file: " << file );
vCap = VideoCapture( file );
}
else
{
INFO_STREAM( "Attempting to capture from device: " << device );
vCap = VideoCapture( device );
// Read a first frame often empty in camera
Mat img;
vCap >> img;
}
if( !vCap.isOpened() ) FATAL_STREAM( "Failed to open video source" );
else INFO_STREAM( "Device or file opened");
Mat img;
vCap >> img;
// If no dimensions defined, do not do any resizing
if(dimx == 0 || dimy == 0)
{
dimx = img.cols;
dimy = img.rows;
}
// If optical centers are not defined just use center of image
if(cx == 0 || cy == 0)
{
cx = dimx / 2.0f;
cy = dimy / 2.0f;
}
// Creating output files
std::ofstream posesFile;
if(!outposes.empty())
{
posesFile.open (outposes[f_n]);
}
std::ofstream featuresFile;
if(!outfeatures.empty())
{
featuresFile.open(outfeatures[f_n]);
}
int frameProc = 0;
// faces in a row detected
int facesInRow = 0;
// saving the videos
VideoWriter writerFace;
if(!outvideos.empty())
{
writerFace = VideoWriter(outvideos[f_n], CV_FOURCC('D','I','V','X'), 30, img.size(), true);
}
// Variables useful for the tracking itself
bool success = false;
bool trackingInitialised = false;
// For measuring the timings
int64 t1,t0 = cv::getTickCount();
double fps = 10;
Mat disp;
INFO_STREAM( "Starting tracking");
while(!img.empty())
{
Mat_<float> depth;
Mat_<uchar> gray;
cvtColor(img, gray, CV_BGR2GRAY);
// Don't resize if it's unneeded
Mat_<uchar> img_scaled;
if(dimx != gray.cols || dimy != gray.rows)
{
resize( gray, img_scaled, Size( dimx, dimy ) );
resize(img, disp, Size( dimx, dimy));
}
else
{
img_scaled = gray;
disp = img.clone();
}
namedWindow("colour",1);
// Get depth image
if(readDepth)
{
char* dst = new char[100];
std::stringstream sstream;
//sstream << dDir << "\\depth%06d.png";
sstream << dDirs[f_n] << "\\depth%05d.png";
sprintf(dst, sstream.str().c_str(), frameProc + 1);
Mat_<short> dImg = imread(string(dst), -1);
if(!dImg.empty())
{
if(dimx != dImg.cols || dimy != dImg.rows)
{
Mat_<short> dImgT;
resize(dImg, dImgT, Size( dimx, dimy));
dImgT.convertTo(depth, CV_32F);
}
else
{
dImg.convertTo(depth, CV_32F);
}
}
else
{
WARN_STREAM( "Can't find depth image" );
}
}
Vec6d poseEstimateHaar;
Matx66d poseEstimateHaarUncertainty;
Rect faceRegion;
// The start place where CLM should start a search (or if it fails, can use the frame detection)
if(!trackingInitialised || (!success && ( frameProc % 2 == 0)))
{
INFO_STREAM( "Attempting to initialise a face");
// The tracker can return multiple head pose observation
vector<Vec6d> poseEstimatesInitialiser;
vector<Matx66d> covariancesInitialiser;
vector<Rect> regionsInitialiser;
bool initSuccess = PoseDetectorHaar::InitialisePosesHaar(img_scaled, depth, poseEstimatesInitialiser, covariancesInitialiser, regionsInitialiser, classifier, fx, fy, cx, cy, haarParams);
if(initSuccess)
{
INFO_STREAM( "Face(s) detected");
if(poseEstimatesInitialiser.size() > 1)
{
cout << "ambiguous detection: " << endl;
// keep the closest one (this is a hack for the experiment)
double best = 10000;
int bestIndex = -1;
for( size_t i = 0; i < poseEstimatesInitialiser.size(); ++i)
{
cout << regionsInitialiser[i].x << " " << regionsInitialiser[i].y << " " << regionsInitialiser[i].width << " " << regionsInitialiser[i].height << endl;
if(poseEstimatesInitialiser[i][2] < best && poseEstimatesInitialiser[i][2] > 100)
{
bestIndex = i;
best = poseEstimatesInitialiser[i][2];
}
}
if(bestIndex != -1)
{
cout << "Choosing bbox:" << regionsInitialiser[bestIndex].x << " " << regionsInitialiser[bestIndex].y << " " << regionsInitialiser[bestIndex].width << " " << regionsInitialiser[bestIndex].height << endl;
faceRegion = regionsInitialiser[bestIndex];
}
else
{
initSuccess = false;
}
}
else
{
faceRegion = regionsInitialiser[0];
}
facesInRow++;
}
}
// If condition for tracking is met initialise the trackers
if(!trackingInitialised && facesInRow >= 1)
{
INFO_STREAM( "Initialising CLM");
trackingInitialised = CLMWrapper::InitialiseCLM(img_scaled, depth, clmModel, poseEstimateHaar, faceRegion, fx, fy, cx, cy, clmParams);
facesInRow = 0;
}
// opencv detector is needed here, if tracking failed reinitialise using it
if(trackingInitialised)
{
success = CLMWrapper::TrackCLM(img_scaled, depth, clmModel, vector<Vec6d>(), vector<Matx66d>(), faceRegion, fx, fy, cx, cy, clmParams);
}
if(success)
{
clmParams.wSizeCurrent = clmParams.wSizeSmall;
}
else
{
clmParams.wSizeCurrent = clmParams.wSizeInit;
}
// Changes for no reinit version
//success = true;
//clmParams.wSizeCurrent = clmParams.wSizeInit;
Vec6d poseEstimateCLM = CLMWrapper::GetPoseCLM(clmModel, fx, fy, cx, cy, clmParams);
if(!outfeatures.empty())
{
featuresFile << frameProc + 1 << " " << success;
for (int i = 0; i < 66 * 2; ++ i)
{
featuresFile << " " << clmModel._shape.at<double>(i) << endl;
}
featuresFile << endl;
}
if(!outposes.empty())
{
posesFile << frameProc + 1 << " " << (float)frameProc * 1000/30 << " " << 1 << " " << poseEstimateCLM[0] << " " << poseEstimateCLM[1] << " " << poseEstimateCLM[2] << " " << poseEstimateCLM[3] << " " << poseEstimateCLM[4] << " " << poseEstimateCLM[5] << endl;
}
if(success)
{
int idx = clmModel._clm.GetViewIdx();
// drawing the facial features on the face if tracking is successful
clmModel._clm._pdm.Draw(disp, clmModel._shape, clmModel._clm._triangulations[idx], clmModel._clm._visi[0][idx]);
DrawBox(disp, poseEstimateCLM, Scalar(255,0,0), 3, fx, fy, cx, cy);
}
else if(!clmModel._clm._pglobl.empty())
{
int idx = clmModel._clm.GetViewIdx();
// draw the facial features
clmModel._clm._pdm.Draw(disp, clmModel._shape, clmModel._clm._triangulations[idx], clmModel._clm._visi[0][idx]);
// if tracking fails draw a red outline
DrawBox(disp, poseEstimateCLM, Scalar(0,0,255), 3, fx, fy, cx, cy);
}
if(frameProc % 10 == 0)
{
t1 = cv::getTickCount();
fps = 10.0 / (double(t1-t0)/cv::getTickFrequency());
t0 = t1;
}
char fpsC[255];
sprintf(fpsC, "%d", (int)fps);
string fpsSt("FPS:");
fpsSt += fpsC;
cv::putText(disp, fpsSt, cv::Point(10,20), CV_FONT_HERSHEY_SIMPLEX, 0.5, CV_RGB(255,0,0));
frameProc++;
imshow("colour", disp);
if(!depth.empty())
{
imshow("depth", depth/2000.0);
}
vCap >> img;
if(!outvideos.empty())
{
writerFace << disp;
}
// detect key presses
char c = cv::waitKey(2);
// key detections
// restart the tracker
if(c == 'r')
{
trackingInitialised = false;
facesInRow = 0;
}
// quit the application
else if(c=='q')
{
return(0);
}
}
trackingInitialised = false;
facesInRow = 0;
posesFile.close();
// break out of the loop if done with all the files
if(f_n == files.size() -1)
{
done = true;
}
}
return 0;
}
int main (int argc, char **argv)
{
//Convert arguments to more convenient vector form
vector<string> arguments = get_arguments(argc, argv);
// Some initial parameters that can be overriden from command line
vector<string> files, dFiles, outimages, outfeaturess;
// these can be overriden using -cx, -cy, -fx, -fy, -dimx, -dimy flags
float fx = 500, fy = 500, cx = 0, cy = 0;
int dimx = 0, dimy = 0;
// initial translation, rotation and scale (can be specified by the user)
vector<Vec6d> initial_poses;
bool useCLMTracker = true;
// Get camera parameters
CLMWrapper::get_camera_params(fx, fy, cx, cy, dimx, dimy, arguments);
CLMWrapper::get_image_input_output_params(files, dFiles, outfeaturess, outimages, initial_poses, arguments);
CLMWrapper::CLMParameters clmParams(arguments);
// The modules that are being used for tracking
CLMTracker::TrackerCLM clmModel;
cout << "Loading the model" << endl;
clmModel.Read(clmParams.defaultModelLoc, clmParams.override_pdm_loc);
cout << "Model loaded" << endl;
PoseDetectorHaar::PoseDetectorHaarParameters haarParams;
haarParams.ClassifierLocation = "classifiers/haarcascade_frontalface_alt2.xml";
CascadeClassifier classifier(haarParams.ClassifierLocation);
bool visualise = true;
//clmParams.multi_view = true;
// Do some image loading
for(size_t i = 0; i < files.size(); i++)
{
string file = files.at(i);
// Loading image
Mat img = imread(file, -1);
// Loading depth file if exists
Mat dTemp;
Mat_<float> dImg;
if(dFiles.size() > 0)
{
string dFile = dFiles.at(i);
dTemp = imread(dFile, -1);
dTemp.convertTo(dImg, CV_32F);
}
if(dimx != 0)
{
cv::resize(img.clone(), img, cv::Size(dimx, dimy));
if(!dImg.empty())
{
cv::resize(dImg.clone(), dImg, cv::Size(dimx, dimy));
}
}
bool trackingInitialised = false;
// Making sure the image is in uchar grayscale
Mat_<uchar> gray;
convert_to_grayscale(img, gray);
// Face detection initialisation
Vec6d poseEstimateHaar;
Matx66d poseEstimateHaarUncertainty;
vector<Vec6d> poseEstimatesInitialiser;
vector<Matx66d> covariancesInitialiser;
vector<Rect> faceRegions;
bool initSuccess = false;
// if no pose defined we just use OpenCV
if(initial_poses.empty())
{
initSuccess = PoseDetectorHaar::InitialisePosesHaar(gray, dImg, poseEstimatesInitialiser, covariancesInitialiser, faceRegions, classifier, fx, fy, cx, cy, haarParams);
if(initSuccess)
{
if(poseEstimatesInitialiser.size() > 1)
{
cout << "Multiple faces detected" << endl;
}
}
if(initSuccess)
{
// perform landmark detection for every face detected
for(int r=0; r < faceRegions.size(); ++r)
{
if(!clmParams.multi_view)
{
Vec6d pose(0.0);
bool success = CLMWrapper::InitialiseCLM(gray, dImg, clmModel, pose, faceRegions[r], fx, fy, cx, cy, clmParams);
}
else
{
vector<Vec3d> orientations;
// Have multiple hypotheses, check which one is best and keep it
orientations.push_back(Vec3d(0,0,0));
orientations.push_back(Vec3d(0,0.5236,0));
orientations.push_back(Vec3d(0,-0.5236,0));
orientations.push_back(Vec3d(0.5236,0,0));
orientations.push_back(Vec3d(-0.5236,0,0));
double best_lhood;
Mat best_shape;
for (size_t p = 0; p<orientations.size(); ++p)
{
Vec6d pose;
pose(1) = orientations[p][0];
pose(2) = orientations[p][1];
pose(3) = orientations[p][2];
CLMWrapper::InitialiseCLM(gray, dImg, clmModel, pose, faceRegions[r], fx, fy, cx, cy, clmParams);
double curr_lhood = clmModel._clm._likelihood;
if(p==0 || curr_lhood > best_lhood)
{
best_lhood = curr_lhood;
best_shape = clmModel._shape.clone();
}
}
clmModel._shape = best_shape;
clmModel._clm._likelihood = best_lhood;
}
// Writing out the detected landmarks
if(!outfeaturess.empty())
{
char name[100];
sprintf(name, "_det_%d", r);
boost::filesystem::path slash("/");
std::string preferredSlash = slash.make_preferred().string();
// append detection number
boost::filesystem::path out_feat_path(outfeaturess.at(i));
boost::filesystem::path dir = out_feat_path.parent_path();
boost::filesystem::path fname = out_feat_path.filename().replace_extension("");
boost::filesystem::path ext = out_feat_path.extension();
string outfeatures = dir.string() + preferredSlash + fname.string() + string(name) + ext.string();
write_out_landmarks(outfeatures, clmModel);
}
// displaying detected stuff
Mat disp;
create_display_image(img, disp, clmModel);
if(visualise)
{
// For debug purposes show final likelihood
//std::ostringstream s;
//s << clmModel._clm._likelihood;
//string lhoodSt("Lhood:");
//lhoodSt += s.str();
//cv::putText(disp, lhoodSt, cv::Point(10,20), CV_FONT_HERSHEY_SIMPLEX, 0.5, CV_RGB(255,0,0));
imshow("colour", disp);
cv::waitKey(10);
}
if(!outimages.empty())
{
string outimage = outimages.at(i);
if(!outimage.empty())
{
char name[100];
sprintf(name, "_det_%d", r);
boost::filesystem::path slash("/");
std::string preferredSlash = slash.make_preferred().string();
// append detection number
boost::filesystem::path out_feat_path(outimage);
boost::filesystem::path dir = out_feat_path.parent_path();
boost::filesystem::path fname = out_feat_path.filename().replace_extension("");
boost::filesystem::path ext = out_feat_path.extension();
outimage = dir.string() + preferredSlash + fname.string() + string(name) + ext.string();
imwrite(outimage, disp);
}
}
}
}
else
{
cout << "No faces detected" << endl;
continue;
}
}
else
{
initSuccess = true;
// if pose defined we don't need the conventional initialisation
if(!clmParams.multi_view)
{
clmModel._clm._pglobl = Mat(initial_poses[i]);
Vec6d pose = CLMWrapper::GetPoseCLM(clmModel, dImg, fx, fy, cx, cy, clmParams);
bool success = CLMWrapper::InitialiseCLM(gray, dImg, clmModel, pose, Rect(), fx, fy, cx, cy, clmParams);
}
else
{
vector<Vec3d> orientations;
// Have multiple hypotheses, check which one is best and keep it
orientations.push_back(Vec3d(0,0,0));
orientations.push_back(Vec3d(0,0.5236,0));
orientations.push_back(Vec3d(0,-0.5236,0));
orientations.push_back(Vec3d(0.5236,0,0));
orientations.push_back(Vec3d(-0.5236,0,0));
double best_lhood;
Mat best_shape;
for (size_t p = 0; p<orientations.size(); ++p)
{
clmModel._clm._pglobl = Mat(initial_poses[i]);
clmModel._clm._pglobl.at<double>(1) = orientations[p][0];
clmModel._clm._pglobl.at<double>(2) = orientations[p][1];
clmModel._clm._pglobl.at<double>(3) = orientations[p][2];
Vec6d pose = CLMWrapper::GetPoseCLM(clmModel, dImg, fx, fy, cx, cy, clmParams);
CLMWrapper::InitialiseCLM(gray, dImg, clmModel, pose, Rect(), fx, fy, cx, cy, clmParams);
double curr_lhood = clmModel._clm._likelihood;
if(p==0 || curr_lhood > best_lhood)
{
best_lhood = curr_lhood;
best_shape = clmModel._shape.clone();
}
}
clmModel._shape = best_shape;
}
// Writing out the detected landmarks
if(!outfeaturess.empty())
{
string outfeatures = outfeaturess.at(i);
write_out_landmarks(outfeatures, clmModel);
}
// displaying detected stuff
Mat disp;
create_display_image(img, disp, clmModel);
if(visualise)
{
imshow("colour", disp);
cv::waitKey(5);
}
if(!outimages.empty())
{
string outimage = outimages.at(i);
if(!outimage.empty())
{
imwrite(outimage, disp);
}
}
}
// Reset the parameters if more images are coming
if(files.size() > 0)
{
clmModel._clm._plocal.setTo(0);
}
}
return 0;
}
int main(int argc, char **argv) {
try {
ParseArgs(argc, argv);
int num_cameras = static_cast<int>(imgs.size());
if (num_cameras < 1)
throw runtime_error("Need at least one camera");
// Find features
cout << "Finding features...\n";
Ptr<detail::FeaturesFinder> features_finder = features_finder_creator->Create();
for (int i = 0; i < num_cameras; ++i) {
int64 t = getTickCount();
cout << "Finding features in '" << img_names[i] << "'... ";
Ptr<detail::ImageFeatures> features = new detail::ImageFeatures();
(*features_finder)(imgs[i], *features);
features_collection[i] = features;
cout << "#features = " << features_collection.find(i)->second->keypoints.size()
<< ", time = " << (getTickCount() - t) / getTickFrequency() << " sec\n";
}
// Match all pairs
cout << "Matching pairs... ";
MatchesCollection matches_collection;
Ptr<detail::FeaturesMatcher> matcher = features_matcher_creator.Create();
FeaturesCollection::iterator from_iter = features_collection.begin();
FeaturesCollection::iterator from_next_iter = from_iter; ++from_next_iter;
FeaturesCollection::iterator to_iter;
for (; from_next_iter != features_collection.end(); from_iter = from_next_iter++) {
for (to_iter = from_next_iter; to_iter != features_collection.end(); ++to_iter) {
cout << "(" << from_iter->first << "->" << to_iter->first << ") ";
detail::MatchesInfo mi;
(*matcher)(*(from_iter->second), *(to_iter->second), mi);
matches_collection[make_pair(from_iter->first, to_iter->first)]
= new vector<DMatch>(mi.matches);
}
}
cout << endl;
// Estimate homographies
HomographiesP2 Hs;
HomographiesP2 good_Hs;
vector<Mat> Hs_from_0;
RelativeConfidences rel_confs;
Mat keypoints1, keypoints2;
cout << "Estimating Hs...\n";
for (int from = 0; from < num_cameras - 1; ++from) {
for (int to = from + 1; to < num_cameras; ++to) {
const vector<DMatch> &matches = *(matches_collection.find(make_pair(from, to))->second);
cout << "Estimating H between '" << img_names[from] << "' and '" << img_names[to]
<< "'... #matches = " << matches.size();
if (static_cast<int>(matches.size()) < min_num_matches) {
cout << ", not enough matches\n";
continue;
}
ExtractMatchedKeypoints(*(features_collection.find(from)->second),
*(features_collection.find(to)->second),
matches, keypoints1, keypoints2);
vector<uchar> inliers_mask;
Mat_<double> H = findHomography(keypoints1.reshape(2), keypoints2.reshape(2),
inliers_mask, RANSAC, H_est_thresh);
if (H.empty()) {
cout << ", can't estimate H\n";
continue;
}
Ptr<vector<DMatch> > inliers = new vector<DMatch>();
for (size_t i = 0; i < matches.size(); ++i)
if (inliers_mask[i])
inliers->push_back(matches[i]);
cout << ", #inliers = " << inliers->size();
double rms_err = 0;
for (size_t i = 0; i < matches.size(); ++i) {
const Point2d &kp1 = keypoints1.at<Point2d>(0, i);
const Point2d &kp2 = keypoints2.at<Point2d>(0, i);
double x = H(0, 0) * kp1.x + H(0, 1) * kp1.y + H(0, 2);
double y = H(1, 0) * kp1.x + H(1, 1) * kp1.y + H(1, 2);
double z = H(2, 0) * kp1.x + H(2, 1) * kp1.y + H(2, 2);
x /= z; y /= z;
rms_err += (kp2.x - x) * (kp2.x - x) + (kp2.y - y) * (kp2.y - y);
}
rms_err = sqrt(rms_err / matches.size());
cout << ", RMS err = " << rms_err;
// See "Automatic Panoramic Image Stitching using Invariant Features"
// by Matthew Brown and David G. Lowe, IJCV 2007 for the explanation
double confidence = inliers->size() / (8 + 0.3 * matches.size()) - 1;
rel_confs[make_pair(from, to)] = confidence;
cout << ", conf = " << confidence;
cout << endl;
Hs[make_pair(from, to)] = H;
matches_collection[make_pair(from, to)] = inliers;
if (confidence > 0)
good_Hs[make_pair(from, to)] = H;
if (from == 0)
Hs_from_0.push_back(H);
}
}
// Linear calibration
if (K_init.empty()) {
cout << "Linear calibrating...\n";
if (lin_est_skew)
K_init = CalibRotationalCameraLinear(good_Hs);
else
K_init = CalibRotationalCameraLinearNoSkew(good_Hs);
cout << "K_init =\n" << K_init << endl;
}
// Non-linear refinement
cout << "Refining camera...\n";
if (Hs_from_0.size() != num_cameras - 1) {
stringstream msg;
msg << "Refinement requires Hs between first and all other images, "
<< "but only " << Hs_from_0.size() << " were/was found";
throw runtime_error(msg.str());
}
map<int, Mat> Rs;
Rs[0] = Mat::eye(3, 3, CV_64F);
for (int i = 1; i < num_cameras; ++i)
Rs[i] = K_init.inv() * Hs_from_0[i - 1] * K_init;
Mat_<double> K_refined = K_init.clone();
if (refine_skew)
RefineRigidCamera(K_refined, Rs, features_collection, matches_collection);
else {
K_refined(0, 1) = 0;
RefineRigidCamera(K_refined, Rs, features_collection, matches_collection,
REFINE_FLAG_K_ALL & ~REFINE_FLAG_K_SKEW);
}
cout << "K_refined =\n" << K_refined << endl;
cout << "SUMMARY\n";
cout << "K_init =\n" << K_init << endl;
cout << "K_refined =\n" << K_refined << endl;
}
catch (const exception &e) {
cout << "Error: " << e.what() << endl;
}
return 0;
}