Ask Your Question

socratic's profile - activity

2019-09-16 03:06:42 -0600 received badge  Notable Question (source)
2019-01-17 14:47:55 -0600 received badge  Popular Question (source)
2018-06-07 10:03:59 -0600 received badge  Nice Question (source)
2017-05-09 08:01:22 -0600 received badge  Enthusiast
2017-05-09 03:23:54 -0600 received badge  Student (source)
2017-05-08 15:44:38 -0600 asked a question Error in gauss-newton implementation for pose optimization

Hello. I’m using a modified version of a gauss-newton method to refine a pose estimate using OpenCV. The unmodified code can be found here:

The details of this approach are outlined in the corresponding paper:

Marchand, Eric, Hideaki Uchiyama, and Fabien Spindler. "Pose estimation for augmented reality: a hands-on survey." IEEE transactions on visualization and computer graphics 22.12 (2016): 2633-2651.

A PDF can be found here:

The part that is relevant (Pages 4 and 5) are screencapped below:

Gauss-Newton Minimization

Here is what I have done. First, I’ve (hopefully) “corrected” some errors: (a) dt and dR can be passed by reference to exponential_map() (even though cv::Mat is essentially a pointer). (b) The last entry of each 2x6 Jacobian matrix,<double>(i*2+1,5), was -x[i].y but should be -x[i].x. (c) I’ve also tried using a different formula for the projection. Specifically, one that includes the focal length and principal point:<double>(i*2,0)   = cx + fx *<double>(0,0) /<double>(2,0);<double>(i*2+1,0) = cy + fy *<double>(1,0) /<double>(2,0);

Here is the relevant code I am using, in its entirety (control starts at optimizePose3()):

void exponential_map(const cv::Mat &v, cv::Mat &dt, cv::Mat &dR)
  double vx =<double>(0,0);
  double vy =<double>(1,0);
  double vz =<double>(2,0);
  double vtux =<double>(3,0);
  double vtuy =<double>(4,0);
  double vtuz =<double>(5,0);
  cv::Mat tu = (cv::Mat_<double>(3,1) << vtux, vtuy, vtuz); // theta u
  cv::Rodrigues(tu, dR);
  double theta = sqrt(;
  double sinc = (fabs(theta) < 1.0e-8) ? 1.0 : sin(theta) / theta;
  double mcosc = (fabs(theta) < 2.5e-4) ? 0.5 : (1.-cos(theta)) / theta / theta;
  double msinc = (fabs(theta) < 2.5e-4) ? (1./6.) : (1.-sin(theta)/theta) / theta / theta;<double>(0,0) = vx*(sinc + vtux*vtux*msinc)
                     + vy*(vtux*vtuy*msinc - vtuz*mcosc)
                     + vz*(vtux*vtuz*msinc + vtuy*mcosc);<double>(1,0) = vx*(vtux*vtuy*msinc + vtuz*mcosc)
                     + vy*(sinc + vtuy*vtuy*msinc)
                     + vz*(vtuy*vtuz*msinc - vtux*mcosc);<double>(2,0) = vx*(vtux*vtuz*msinc - vtuy*mcosc)
                     + vy*(vtuy*vtuz*msinc + vtux*mcosc)
                     + vz*(sinc + vtuz*vtuz*msinc);

void optimizePose3(const PoseEstimation &pose, 
                   std::vector<FeatureMatch> &feature_matches,
                   PoseEstimation &optimized_pose) {

  //Set camera parameters
  double fx =<double>(0, 0); //Focal length
  double fy =<double>(1, 1);
  double cx =<double>(0, 2); //Principal point
  double cy =<double>(1, 2);

  auto inlier_matches = getInliers(pose, feature_matches);

  std::vector<cv::Point3d> wX;
  std::vector<cv::Point2d> x;

  const unsigned int npoints = inlier_matches.size();
  cv::Mat J(2*npoints, 6, CV_64F);
  double lambda = 0.25;
  cv::Mat xq(npoints*2, 1, CV_64F);
  cv::Mat xn(npoints*2, 1 ...
2017-01-05 12:54:55 -0600 received badge  Critic (source)
2016-10-28 11:09:10 -0600 commented answer How to use cv::flann (radiusSearch) to find all neighbouring points within radius r in 2D using Euclidean distance

OK. Thanks again for your help.

2016-10-28 09:23:21 -0600 commented answer How to use cv::flann (radiusSearch) to find all neighbouring points within radius r in 2D using Euclidean distance

Thank you so much for your response. Is there any way to avoid specifying the number of neighbours to find? The trailing zeros take up memory, and it won't be clear if the indices indicate the first element or no-more-elements.

2016-10-28 09:07:16 -0600 received badge  Scholar (source)
2016-10-28 09:07:14 -0600 received badge  Supporter (source)
2016-10-27 21:59:01 -0600 asked a question How to use cv::flann (radiusSearch) to find all neighbouring points within radius r in 2D using Euclidean distance

I have a std::vector of a couple million points (cv::Point2d) and I'd like to find, for every point, all other points within a 2 pixel radius.

Since my project already requires OpenCV, I thought it would be useful to use the cv::flann module. However, I haven't made much progress with my attempts so far. In particular, I'm not sure how to present my data to the index constructor. Here is my best attempt:

#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/flann.hpp>

int main(int argc, char* argv[]) {

  // Sample data
  std::vector<cv::Point2d> points;

  // Create an empty Mat for the features that includes dimensional
  // space for an x and y coordinate
  cv::Mat_<double> features(0,2);

  for(auto && point : points) {

    //Fill matrix
    cv::Mat row = (cv::Mat_<double>(1, 2) << point.x, point.y);
  std::cout << features << std::endl;

  cv::flann::Index flann_index(features, cv::flann::KDTreeIndexParams(1));

  unsigned int max_neighbours = 10;
  cv::Mat query = (cv::Mat_<double>(1, 2) << 313.0, 245.6);
  //Ensure indices and dists have enough space for at least max_neighbours
  cv::Mat_<double> indices(max_neighbours, 2) ;
  cv::Mat_<double> dists(max_neighbours, 2);
  double radius= 2.0;

  flann_index.radiusSearch(query, indices, dists, radius, max_neighbours,

The code compiles, but I receive a runtime error:

OpenCV Error: Unsupported format or combination of formats (type=6
) in buildIndex_, file /opt/opencv/modules/flann/src/miniflann.cpp, line 315
terminate called after throwing an instance of 'cv::Exception'
  what():  /opt/opencv/modules/flann/src/miniflann.cpp:315: error: (-210) type=6
 in function buildIndex_

Aborted (core dumped)

Can anyone please help me figure out how to use radiusSearch? And how can I configure the radiusSearch to find all neighbours, instead of just max_neighbours? Thanks in advance.