void VilDraw::draw_cross(vil_image_view<vxl_byte> &image,
const vcl_vector< vgl_point_2d<double>> &pts,
int crossWidth,
const vcl_vector< vxl_byte >& colour, int lineWidth)
{
assert(image.nplanes() == 3 || image.nplanes() == 1);
assert(colour.size() == 3);
vil_image_view<vxl_byte> rgb_image = image;
if (image.nplanes() == 1) {
image = VilUtil::gray_2_rgb(image);
}
for (unsigned int i = 0; i<pts.size(); i++)
{
//center point
double px = pts[i].x();
double py = pts[i].y();
vgl_point_2d<double> p1, p2, p3, p4;
double h_l = crossWidth;
p1.set(px - h_l, py);
p2.set(px + h_l, py);
p3.set(px, py - h_l);
p4.set(px, py + h_l);
VilAlgoPlus::fill_line(image, p1, p2, colour);
VilAlgoPlus::fill_line(image, p3, p4, colour);
}
}
bool VilGMM::train(const vcl_vector<vnl_vector<double> > & data)
{
vpdfl_gaussian_builder g_builder;
vpdfl_mixture_builder builder;
builder.init(g_builder,comp_n_);
builder.set_weights_fixed(false);
if (data.size() <= comp_n_ * 20) {
return false;
}
if (gmm_) {
delete gmm_;
gmm_ = NULL;
}
gmm_ = builder.new_model();
mbl_data_array_wrapper<vnl_vector<double> > data_array(data);
builder.build(*gmm_, data_array);
if (verbose_) {
vcl_cout<<"training sample number is "<<data.size()<<vcl_endl;
vcl_cout<<"Probability distribution function is "<<gmm_<<vcl_endl;
vcl_cout<<"Mean: "<<gmm_->mean()<<vcl_endl;
vcl_cout<<"Var: "<<gmm_->variance()<<vcl_endl;
}
return true;
}
bool VglPlus::intersectionFromLineSet(const vcl_vector<vgl_line_2d<double> > & lines, vgl_point_2d<double> & orthocenter)
{
assert(lines.size() >= 2);
int num = 0;
double px = 0.0;
double py = 0.0;
for (int i = 0; i<lines.size(); i++) {
for (int j = i+1; j<lines.size(); j++) {
vgl_point_2d<double> p;
bool isIntersect = vgl_intersection(lines[i], lines[j], p);
if (isIntersect) {
px += p.x();
py += p.y();
num++;
}
}
}
if (num >= 1) {
orthocenter = vgl_point_2d<double>(px/num, py/num);
return true;
}
else
{
return false;
}
}
bool rgrsn_ldp::transition_matrix(const vcl_vector<int> & fns,
const vcl_vector<double> & values,
vnl_matrix<double> & transition,
const double resolution)
{
assert(fns.size() == values.size());
double min_v = *vcl_min_element(values.begin(), values.end());
double max_v = *vcl_max_element(values.begin(), values.end());
unsigned num_bin = (max_v - min_v)/resolution;
transition = vnl_matrix<double>(num_bin, num_bin, 0.0);
vnl_vector<double> column(num_bin, 0.0);
for (int i = 0; i<fns.size()-1; i++) {
if (fns[i] + 1 == fns[i+1]) {
double cur_v = values[i];
double next_v = values[i+1];
unsigned cur_bin = value_to_bin_number(min_v, resolution, cur_v, num_bin);
unsigned next_bin = value_to_bin_number(min_v, resolution, next_v, num_bin);
transition[next_bin][cur_bin] += 1.0;
column[cur_bin] += 1.0;
}
}
// normalize each column
for (int r = 0; r < transition.rows(); r++) {
for (int c = 0; c < transition.cols(); c++) {
transition[r][c] /= column[c];
}
}
return true;
}
bool VxlMvlPlus::fundamental_RANSAC(vcl_vector< vgl_point_2d< double > > const & positions1,
vcl_vector< vgl_point_2d< double > > const & positions2,
vcl_vector< vcl_pair<int, int> > const & initialMatchedIndices, // matched index first --> second
double error_threshold,
// output
vcl_vector< vcl_pair<int, int> > & finalMatchedIndices,
vpgl_fundamental_matrix<double> & F)
{
bool isFind = false;
// extract point pair from initial matches
vcl_vector<vgl_point_2d<double> > pts1_matched;
vcl_vector<vgl_point_2d<double> > pts2_matched;
for (int i = 0; i<initialMatchedIndices.size(); i++) {
pts1_matched.push_back(positions1[initialMatchedIndices[i].first]);
pts2_matched.push_back(positions2[initialMatchedIndices[i].second]);
}
vcl_vector<bool> inliers;
isFind = VxlMvlPlus::fundamental_RANSAC(pts1_matched, pts2_matched, inliers, error_threshold, F);
assert(inliers.size() == initialMatchedIndices.size());
if (isFind) {
for (int i = 0; i<inliers.size(); i++) {
if (inliers[i]) {
finalMatchedIndices.push_back(initialMatchedIndices[i]);
}
}
}
printf("fundamental matrix RANSAC find %lu from %lu initial matchings\n", finalMatchedIndices.size(), initialMatchedIndices.size());
return isFind;
}
bool VilBaplSIFT::writeSIFT(const char *file, const vcl_vector< bapl_keypoint_sptr > & keypoints)
{
FILE *pf = fopen(file, "w");
if (!pf) {
printf("Error: canot open %s\n", file);
return false;
}
int n = (int)keypoints.size();
int len = 128;
fprintf(pf, "%d\t %d\n", n, len);
for (int i = 0; i<keypoints.size(); i++) {
bapl_lowe_keypoint_sptr sift = dynamic_cast<bapl_lowe_keypoint*>(keypoints[i].as_pointer());
double loc_x = sift->location_i();
double loc_y = sift->location_j();
double scale = sift->scale();
double orientation = sift->orientation();
fprintf(pf, "%lf\t %lf\t %lf\t %lf\n", loc_x, loc_y, scale, orientation);
vnl_vector_fixed<double,128> feat = sift->descriptor();
for (int j = 0; j<feat.size(); j++) {
fprintf(pf, "%lf ", feat[j]);
}
fprintf(pf, "\n");
}
fclose(pf);
printf("save sift to %s\n", file);
return true;
}
void VglPlus::loadPointVector(const char *fileName, vcl_string &imageName, vcl_vector<vgl_point_2d<double>> &wldPts, vcl_vector< vgl_point_2d<double> > &imgPts)
{
assert(wldPts.size() == 0);
assert(imgPts.size() == 0);
vsl_b_ifstream bfs_in(fileName);
assert(bfs_in.is().good() == true);
char strName[1024] = {NULL};
vsl_b_read(bfs_in, strName);
imageName = vcl_string(strName);
int num = 0;
vsl_b_read(bfs_in, num);
wldPts.resize(num);
imgPts.resize(num);
for (int i = 0; i<num; i++) {
vsl_b_read(bfs_in, wldPts[i]);
}
for (int i = 0; i<num; i++) {
vsl_b_read(bfs_in, imgPts[i]);
}
bfs_in.close();
}
void VilBaplSIFT::getSiftPositions(const vil_image_view<vxl_byte> &image, vcl_vector<vgl_point_2d<double> > &points, double curve_ratio)
{
assert(image.nplanes() == 1 || image.nplanes() == 3);
assert(points.size() == 0);
vil_image_view<vxl_byte> grey_img;
if (image.nplanes() == 3) {
vil_convert_planes_to_grey(image, grey_img);
}
else
{
grey_img.deep_copy(image);
}
vcl_vector<bapl_keypoint_sptr> sift_keypoints;
vil_image_resource_sptr image_sptr = vil_new_image_resource_of_view(grey_img);
// float curve_ratio = 2.0f;
bapl_keypoint_extractor(image_sptr, sift_keypoints, curve_ratio);
vcl_vector<bapl_keypoint_sptr>::iterator keypoint_itr, keypoint_end;
keypoint_end = sift_keypoints.end();
for (keypoint_itr = sift_keypoints.begin(); keypoint_itr != keypoint_end; ++keypoint_itr)
{
bapl_lowe_keypoint_sptr sift_lowe_keypoint = dynamic_cast<bapl_lowe_keypoint*>((*keypoint_itr).as_pointer());
double x = sift_lowe_keypoint->location_i();
double y = sift_lowe_keypoint->location_j();
points.push_back(vgl_point_2d<double>(x, y));
}
}
bool rgrsn_ldp::local_dynamic_programming(const vnl_matrix<double> & probMap, int nNeighborBin,
vcl_vector<int> & optimalBins)
{
const int N = probMap.rows();
const int nBin = probMap.cols();
// dynamic programming
vnl_matrix<double> accumulatedProbMap = vnl_matrix<double>(N, nBin);
accumulatedProbMap.fill(0.0);
vnl_matrix<int> lookbackTable = vnl_matrix<int>(N, nBin);
lookbackTable.fill(0);
// copy first row
for (int c = 0; c<probMap.cols(); c++) {
accumulatedProbMap[0][c] = probMap[0][c];
lookbackTable[0][c] = c;
}
for (int r = 1; r <N; r++) {
for (int c = 0; c<probMap.cols(); c++) {
// lookup all possible place in the window
double max_val = -1;
int max_index = -1;
for (int w = -nNeighborBin; w <= nNeighborBin; w++) {
if (c + w <0 || c + w >= probMap.cols()) {
continue;
}
double val = probMap[r][c] + accumulatedProbMap[r-1][c+w];
if (val > max_val) {
max_val = val;
max_index = c + w; // most probable path from the [r-1] row, in column c + w
}
}
assert(max_index != -1);
accumulatedProbMap[r][c] = max_val;
lookbackTable[r][c] = max_index;
}
}
// lookback the table
double max_prob = -1.0;
int max_prob_index = -1;
for (int c = 0; c<accumulatedProbMap.cols(); c++) {
if (accumulatedProbMap[N-1][c] > max_prob) {
max_prob = accumulatedProbMap[N-1][c];
max_prob_index = c;
}
}
// back track
optimalBins.push_back(max_prob_index);
for (int r = N-1; r > 0; r--) {
int bin = lookbackTable[r][optimalBins.back()];
optimalBins.push_back(bin);
}
assert(optimalBins.size() == N);
// vcl_reverse(optimalBins.begin(), optimalBins.end());
return true;
}
void VilBaplSIFT::getSIFTLocations(const vcl_vector<bapl_keypoint_sptr> & keypoints, vcl_vector<vgl_point_2d<double> > & pts)
{
for (int i = 0; i<keypoints.size(); i++) {
bapl_lowe_keypoint_sptr sift = dynamic_cast<bapl_lowe_keypoint*>(keypoints[i].as_pointer());
vgl_point_2d<double> p(sift->location_i(), sift->location_j());
pts.push_back(p);
}
}
void VglPlus::saveVector(const char *matName, const vcl_vector<double> & data)
{
assert(data.size() > 0);
vnl_matlab_filewrite writer(matName);
vnl_vector<double> dataVec(&data[0], (int)data.size());
writer.write(dataVec, "data");
printf("save to file: %s\n", matName);
}
bool VglPlus::lineEllipseTangencyByProjection(int imageW, int imageH,
const vgl_line_2d<double> & line,
const vgl_ellipse_2d<double> & ellipseInImage,
vgl_point_2d<double> & pt, int sampleNum)
{
vil_image_view<vxl_byte> maskImage(imageW, imageH, 3);
maskImage.fill(0);
VilPlus::draw_line(maskImage, line, VilPlus::white(), 2);
vgl_polygon<double> poly = ellipseInImage.polygon(sampleNum);
assert(poly.num_sheets() == 1);
const vcl_vector< vgl_point_2d< double > > sheet = poly[0];
assert( sheet.size() > 1 );
vcl_vector<vgl_point_2d<double>> pts;
for (int i = 0; i<sheet.size(); i++) {
int x = vnl_math::rnd_halfinttoeven(sheet[i].x());
int y = vnl_math::rnd_halfinttoeven(sheet[i].y());
if (x >= 0 && x < imageW &&
y >= 0 && y < imageH) {
if (maskImage(x, y) == 255) {
pts.push_back(sheet[i]);
}
}
}
if (pts.size() == 0) {
return false;
}
// caluclate average position
double avgX = 0.0;
double avgY = 0.0;
for (int i =0; i<pts.size(); i++) {
avgX += pts[i].x();
avgY += pts[i].y();
}
avgX /= pts.size();
avgY /= pts.size();
double stdX = 0.0;
double stdY = 0.0;
for (int i = 0; i<pts.size(); i++) {
stdX += (avgX - pts[i].x()) * (avgX - pts[i].x());
stdY += (avgY - pts[i].y()) * (avgY - pts[i].y());
}
stdX = sqrt(stdX/pts.size());
stdY = sqrt(stdY/pts.size());
printf("std x, y is %f %f\n", stdX, stdY);
pt = vgl_point_2d<double>(avgX, avgY);
return true;
}
bool VxlMvlPlus::three_view_six_points_calib(const vcl_vector< vcl_vector<vgl_point_2d<double> > > & pointsVec,
vcl_vector< vnl_matrix_fixed<double, 3, 4> > & Pmatrix,
vgl_homg_point_3d<double> & Q)
{
assert(pointsVec.size() == 3);
for (int i = 0; i<3; i++) {
assert(pointsVec[i].size() == 6);
}
mvl_three_view_six_point_structure mvl36;
// set 6 (x, y)
for (int i = 0; i<3; i++) {
for (int j = 0; j<6; j++) {
mvl36.u(i, j) = pointsVec[i][j].x();
mvl36.v(i, j) = pointsVec[i][j].y();
}
}
bool isFind = mvl36.compute();
if (!isFind) {
return false;
}
int validNum = 0;
mvl_three_view_six_point_structure::solution_t slu; // only one solution can be used
for (int i = 0; i<3; i++) {
validNum += (mvl36.solution[i].valid == true ? 1:0);
if (mvl36.solution[i].valid) {
slu = mvl36.solution[i];
}
}
// no ambiguity
if (validNum != 1) {
printf("%d ambituity solutions\n", validNum);
for (int i = 0; i<3; i++) {
if (mvl36.solution[i].valid)
{
for (int j = 0; j<3; j++) {
vcl_cout<<"P is: \n"<<mvl36.solution[i].P[j]<<vcl_endl;
}
}
vcl_cout<<"Q is "<<vgl_point_3d<double>(mvl36.solution[i].Q[0], mvl36.solution[i].Q[1], mvl36.solution[i].Q[2])<<vcl_endl;
}
return false;
}
for (int i = 0; i<3; i++) {
vcl_cout<<"P is: \n"<<slu.P[i]<<vcl_endl;
Pmatrix.push_back(slu.P[i]);
}
vcl_cout<<"Q is: \n"<<slu.Q<<vcl_endl;
Q = vgl_homg_point_3d<double>(slu.Q[0], slu.Q[1], slu.Q[2], slu.Q[3]);
return true;
}
void VglPlus::savePointVector(const char *fileName, vcl_vector<vgl_point_2d<double>> &pts)
{
vsl_b_ofstream bfs_out(fileName);
vcl_cout<<"save point vector, size is "<<pts.size()<<vcl_endl;
for (int i = 0; i<pts.size(); i++) {
vsl_b_write(bfs_out, pts[i]);
}
bfs_out.close();
}
int VglPlus::sampleElliseInImage(const vgl_ellipse_2d<double> & ellipse, int sampleNum,
int imageW, int imageH, vcl_vector<vgl_point_2d<double> > & pts)
{
vgl_polygon<double> poly = ellipse.polygon(sampleNum);
assert(poly.num_sheets() == 1);
const vcl_vector< vgl_point_2d< double > > sheet = poly[0];
for (int i = 0; i<sheet.size(); i++) {
if (VglPlus::vgl_inside_image(sheet[i], imageW, imageH)) {
pts.push_back(sheet[i]);
}
}
return (int)pts.size();
}
void VilBaplSIFT::getMatchingLocations(const vcl_vector<bapl_key_match> & matches, vcl_vector<vgl_point_2d<double> > & pts1, vcl_vector<vgl_point_2d<double> > & pts2)
{
for (int i = 0; i<matches.size(); i++)
{
bapl_lowe_keypoint_sptr s1 = dynamic_cast<bapl_lowe_keypoint*>(matches[i].first.as_pointer());
bapl_lowe_keypoint_sptr s2 = dynamic_cast<bapl_lowe_keypoint*>(matches[i].second.as_pointer());
vgl_point_2d<double> p1(s1->location_i(), s1->location_j());
vgl_point_2d<double> p2(s2->location_i(), s2->location_j());
pts1.push_back(p1);
pts2.push_back(p2);
}
}
vcl_vector<vgl_point_2d<double> > VilBaplSIFT::getSIFTLocations(const vcl_vector<bapl_keypoint_sptr> & keypoints, const vcl_vector<bool> & inlier)
{
assert(inlier.size() == keypoints.size());
vcl_vector<vgl_point_2d<double> > pts;
for (int i = 0; i<keypoints.size(); i++) {
if (inlier[i]) {
bapl_lowe_keypoint_sptr sift = dynamic_cast<bapl_lowe_keypoint*>(keypoints[i].as_pointer());
vgl_point_2d<double> p(sift->location_i(), sift->location_j());
pts.push_back(p);
}
}
return pts;
}
void VilBaplSIFT::getSiftDescription(const vil_image_view<vxl_byte> & image, const vcl_vector<vgl_point_2d<double> > & pts, vcl_vector<bapl_lowe_keypoint_sptr> & sifts)
{
assert(image.nplanes() == 3);
assert(sifts.size() == 0);
vil_image_view<vxl_byte> grey_img;
vil_convert_planes_to_grey(image, grey_img);
bapl_lowe_pyramid_set_sptr pyramid_set = new bapl_lowe_pyramid_set(vil_new_image_resource_of_view(grey_img));
for (unsigned int i = 0; i<pts.size(); i++) {
// bapl_lowe_keypoint keypoint(pyramid_set, pts[i].x(), pts[i].y());
bapl_lowe_keypoint_sptr keypoint = new bapl_lowe_keypoint(pyramid_set, pts[i].x(), pts[i].y());
sifts.push_back(keypoint);
}
}
void VilBaplSIFT::getSiftDescription(const vil_image_view<vxl_byte> &image, vcl_vector<bapl_lowe_keypoint_sptr> &descriptions)
{
assert(image.nplanes() == 1 || image.nplanes() == 3);
vil_image_view<vxl_byte> grey_img;
if (image.nplanes() == 3) {
vil_convert_planes_to_grey(image, grey_img);
}
else
{
grey_img.deep_copy(image);
}
vcl_vector<bapl_keypoint_sptr> sift_keypoints;
vil_image_resource_sptr image_sptr = vil_new_image_resource_of_view(grey_img);
bapl_keypoint_extractor(image_sptr, sift_keypoints);
for (vcl_vector<bapl_keypoint_sptr>::iterator itr = sift_keypoints.begin();
itr != sift_keypoints.end(); itr++) {
bapl_lowe_keypoint_sptr sift_lowe_keypoint = dynamic_cast<bapl_lowe_keypoint*>((*itr).as_pointer());
assert(sift_lowe_keypoint);
descriptions.push_back(sift_lowe_keypoint);
}
}
bool VilBaplSIFT::readSIFT_keypoint(const char *file, vcl_vector< bapl_keypoint_sptr > & keypoints)
{
FILE *pf = fopen(file, "r");
if (!pf) {
printf("Error: canot open %s\n", file);
return false;
}
int n = 0;
int ret = fscanf(pf, "%d ", &n);
assert(ret == 1);
for (int i = 0; i < n; i++) {
double loc_x = 0;
double loc_y = 0;
double scale = 0;
double orientation = 0;
ret = fscanf(pf, "%lf %lf %lf %lf ", &loc_x, &loc_y, &scale, &orientation);
assert(ret == 4);
vnl_vector_fixed<double, 128> desc;
desc.fill(0);
bapl_lowe_pyramid_set_sptr py;
bapl_lowe_keypoint_sptr kp = new bapl_lowe_keypoint(py, loc_x, loc_y, scale, orientation, desc);
keypoints.push_back(kp);
}
fclose(pf);
printf("read %d keypoints.\n", n);
return true;
}
// unknow 4, f, p_3x1
// constraint, 5: up triangle in 3x3 matrix, may have meaning less constraint such as 1 - 1 = 0
// but at least 2 meaningful constraits
calibIdenticalKEqu195_Residual(const vcl_vector< vnl_matrix_fixed<double, 3, 4> > & projections,
const vgl_point_2d<double> & pp):
vnl_least_squares_function(4, 5 * (int)projections.size(), no_gradient),
projections_(projections),
pp_(pp)
{
}
bool VxlSelfCalib::calibIdenticalK(const vcl_vector< vnl_matrix_fixed<double, 3, 4> > & projections,
const vnl_matrix_fixed<double, 3, 3> & initK,
const vnl_vector_fixed<double, 3> & init_p,
vnl_matrix_fixed<double, 3, 3> & finalK,
vnl_vector_fixed<double, 3> & final_p)
{
assert(projections.size() >= 3);
vgl_point_2d<double> pp(initK[0][2], initK[1][2]);
calibIdenticalKEqu195_Residual residual(projections, pp);
vnl_vector<double> x(4, 0.0);
x[0] = initK[0][0];
x[1] = init_p[0];
x[2] = init_p[1];
x[3] = init_p[2];
vnl_levenberg_marquardt lmq(residual);
bool isMinized = lmq.minimize(x);
if (!isMinized) {
vcl_cerr<<"Error: minimization failed.\n";
vcl_cerr<<"x = "<<x<<vcl_endl;
lmq.diagnose_outcome();
return false;
}
lmq.diagnose_outcome();
vcl_cerr<<"x = "<<x<<vcl_endl;
return true;
}
vgl_h_matrix_2d<double> VglPlus::homography(const vcl_vector<vgl_point_2d<double> > &from_pts,
const vcl_vector<vgl_point_2d<double> > &to_pts)
{
assert(from_pts.size() == to_pts.size());
assert(from_pts.size() >= 4);
vcl_vector<vgl_homg_point_2d<double> > pts1;
vcl_vector<vgl_homg_point_2d<double> > pts2;
for (int j = 0; j<from_pts.size(); j++) {
pts1.push_back(vgl_homg_point_2d<double>(from_pts[j].x(), from_pts[j].y(), 1.0));
pts2.push_back(vgl_homg_point_2d<double> (to_pts[j].x(), to_pts[j].y(), 1.0));
}
vgl_h_matrix_2d<double> H = vgl_h_matrix_2d<double>(pts1, pts2);
return H;
}
void vnl_flann::load(const char *index_file, const vcl_vector<vnl_vector<double> > & data)
{
assert(index_file);
const int dim = (int)data.front().size();
// wrap dataset
vnl_matrix<double> data_mat((int)data.size(), (int)data.front().size());
for (int i = 0; i<data.size(); i++) {
assert(data[i].size() == dim);
data_mat.set_row(i, data[i]);
}
Matrix<double> dataset(data_mat.data_block(), (int)data.size(), dim);
SavedIndexParams index_params((std::string(index_file)));
index_ = flann::Index<flann::L2<double>>(dataset, index_params, flann::L2<double>());
dim_ = dim;
}
void vnl_flann::search(const vcl_vector<vnl_vector<double> > & query_data,
vcl_vector<vcl_vector<int> > & indices,
vcl_vector<vcl_vector<double> > & dists,
int knn,
int num_search_leaf) const
{
const int dim = (int)query_data.front().size();
assert(dim == dim_);
// wrap query data
vnl_matrix<double> query_data_mat((int)query_data.size(), (int)query_data.front().size(), dim);
for (int i = 0; i<query_data.size(); i++) {
query_data_mat.set_row(i, query_data[i]);
}
Matrix<double> query_data_wrap(query_data_mat.data_block(), (int)query_data.size(), dim);
index_.knnSearch(query_data_wrap, indices, dists, knn, flann::SearchParams(num_search_leaf));
}
bool VxlMvlPlus::fundamental_RANSAC(vcl_vector< vgl_point_2d< double > > const & first,
vcl_vector< vgl_point_2d< double > > const & second,
vcl_vector< bool > & inlier,
double error_threshold,
vpgl_fundamental_matrix<double> & F)
{
assert(first.size() == second.size());
assert(first.size() >= 4);
vcl_vector<vgl_homg_point_2d<double> > pts1;
vcl_vector<vgl_homg_point_2d<double> > pts2;
for (int i = 0; i<first.size(); i++) {
pts1.push_back((vgl_homg_point_2d<double>)first[i]);
pts2.push_back((vgl_homg_point_2d<double>)second[i]);
}
// distable cout in bapl_bbf_tree construction function
std::streambuf* cerr_sbuf = std::cerr.rdbuf(); // save original sbuf
std::ofstream fout("/dev/null");
std::cerr.rdbuf(fout.rdbuf()); // redirect 'cout' to a 'fout'
FMatrixComputeRANSAC computor(true, error_threshold);
FMatrix f;
bool isF = computor.compute(pts1, pts2, f);
std::cerr.rdbuf(cerr_sbuf); // restore the original stream buffer
if (!isF) {
printf("can't find fundamental matrix\n");
return false;
}
vnl_matrix_fixed<double,3,3> Fmat;
for (int i = 0; i<3; i++) {
for (int j = 0; j<3; j++) {
Fmat[i][j] = f.get(i, j);
}
}
inlier = computor.get_inliers();
assert(inlier.size() == first.size());
F = vpgl_fundamental_matrix<double>(Fmat);
return true;
}
void VglPlus::savePointVector(const char *fileName, const char *imageName, const vcl_vector<vgl_point_2d<double>> &wldPts, const vcl_vector< vgl_point_2d<double> > &imgPts)
{
assert(wldPts.size() == imgPts.size());
vsl_b_ofstream bfs_out(fileName);
vsl_b_write(bfs_out, imageName); // image name
vsl_b_write(bfs_out, (int)wldPts.size()); // pts number
for (int i = 0; i<wldPts.size(); i++) {
vsl_b_write(bfs_out, wldPts[i]);
}
for (int i = 0; i<imgPts.size(); i++) {
vsl_b_write(bfs_out, imgPts[i]);
}
bfs_out.close();
}
// Creates a perspective camera looking at pt, and adds the camera and
// the projection of GOAL to the list.
static void add_pt_and_cam(
vgl_homg_point_3d<double> pt,
vgl_vector_3d<double> trans,
vcl_vector<vgl_point_2d<double> > &points,
vcl_vector<vpgl_perspective_camera<double> > &cameras)
{
vpgl_perspective_camera<double> cam;
cam.look_at(pt);
cam.set_translation(trans);
cameras.push_back(cam);
double x,y;
cam.project(GOAL.x(), GOAL.y(), GOAL.z(), x, y);
std::cout << "x: " << x << " y: " << y << std::endl;
points.push_back(vgl_point_2d<double>(x, y));
}
void CvxLSD::detect_lines(const vil_image_view<vxl_byte> & image, vcl_vector<LSDLineSegment> & line_segments)
{
assert(image.nplanes() == 3 || image.nplanes() == 1);
vil_image_view<vxl_byte> grayImage;
if (image.nplanes() == 3) {
vil_convert_planes_to_grey(image, grayImage);
}
else
{
grayImage = image;
}
double * imageData = NULL;
double * out = NULL;
int width = grayImage.ni();
int height = grayImage.nj();
int n = 0;
imageData = (double *) malloc( width * height * sizeof(double) );
assert(imageData);
for(int j=0; j<height; j++)
{
for(int i=0; i<width; i++)
{
imageData[i + j * width] = grayImage(i, j);
}
}
/* LSD call */
out = lsd(&n, imageData, width, height);
/* print output */
// printf("%d line segments found:\n",n);
line_segments.resize(n);
for(int i=0; i<n; i++)
{
double x1 = out[7*i + 0];
double y1 = out[7*i + 1];
double x2 = out[7*i + 2];
double y2 = out[7*i + 3];
double line_width = out[7*i + 4];
double p = out[7*i + 5];
double tmp = out[7*i + 6];
line_segments[i].seg_ = vgl_line_segment_2d<double>(vgl_point_2d<double>(x1, y1), vgl_point_2d<double>(x2, y2));
line_segments[i].width_ = line_width;
line_segments[i].angle_precision_ = p;
line_segments[i].NFA_ = tmp;
}
free( (void *) imageData );
free( (void *) out );
}
void VglPlus::loadPointVector(const char *fileName, vcl_vector<vgl_point_2d<double>> &pts, int pts_num)
{
vsl_b_ifstream bfs_in(fileName);
assert(bfs_in.is().good() == true);
pts.resize(pts_num);
// && bfs_in.is().eof() != true not work
for (int i = 0; i < pts_num ; i++) {
vsl_b_read(bfs_in, pts[i]);
assert(bfs_in.is().good() == true);
}
bfs_in.close();
}
