Update/sensor noise qual #52

colin-lewis-19 · 2020-06-04T17:04:51Z

Adds a tool for qualifying sensor noise, as well as an implementation of 3d pnp.

rct_optimizations/CMakeLists.txt

rct_examples/src/verification/noise_qualify_2d.cpp

rct_optimizations/CMakeLists.txt

rct_optimizations/include/rct_optimizations/noise_qualifier.h

rct_optimizations/test/noise_qualification_utest.cpp

rct_optimizations/test/pnp_utest.cpp

marip8 · 2020-06-08T20:43:24Z

One more comment: could you move the noise qualification code into a directory inside include and src called validation (instead of verification)? I want to be consistent with #51

rct_optimizations/include/rct_optimizations/validation/noise_qualifier.h

rct_optimizations/test/noise_qualification_utest.cpp

rct_optimizations/test/pnp_utest.cpp

rct_optimizations/src/validation/noise_qualifier.cpp

marip8 · 2020-06-17T21:45:02Z

rct_optimizations/src/validation/noise_qualifier.cpp

+      Eigen::Quaterniond q;
+      Eigen::Matrix3d m = result.camera_to_target.rotation();
+      q = m;
+
+      Eigen::Vector4d v(q.x(), q.y(), q.z(),q.w());
+
+      orientations.push_back(v);


You could reduce with orientations.push_back(Eigen::Quaterniond(result.camera_to_target.rotation()).coeffs())

marip8 · 2020-06-17T21:47:15Z

rct_optimizations/src/validation/noise_qualifier.cpp

+  //Mean quaternion is found using method described by Markley et al: Quaternion Averaging
+  //All quaternions are equally weighted
+
+  //Accumulator Matrix
+  Eigen::Matrix4d A = Eigen::Matrix4d::Zero();
+
+  for (std::size_t i = 0; i <= count; ++i)
+  {
+    //rank 1 update of 'A', the accumulator matrix, with the u,v = q,qT
+    Eigen::Matrix4d temp = (orientations[i] * orientations[i].transpose());
+    A += temp;
+  }
+
+  //scale A
+  A = (1.0/double(count)) * A;
+
+  //Eigenvectors,values should be strictly real
+  Eigen::EigenSolver<Eigen::Matrix4d> E(A, true);
+
+  //Each column of 4x4 vectors is an eigenvector; desired mean has max eigenvalue
+  Eigen::Index max_evi, one;
+
+  //find maximium eigenvalue, and store its index in max_evi
+  E.eigenvalues().real().maxCoeff(&max_evi, &one);
+  Eigen::Vector4d mean = E.eigenvectors().real().col(max_evi);
+
+  output.i.mean = mean[0];
+  output.j.mean = mean[1];
+  output.k.mean = mean[2];
+  output.w.mean = mean[3];
+
+  //Manually calculate standard deviations from mean
+  Eigen::Array4d std_dev = Eigen::Array4d::Zero();
+  Eigen::Array4d sum = Eigen::Array4d::Zero();
+  for (std::size_t i =0; i< count; ++i)
+  {
+   //absolute value taken because a quaternion is equal to its opposite
+   Eigen::Array4d term = orientations[i].cwiseAbs() - mean.cwiseAbs();
+   term = term.square();
+   sum+= term;
+  }
+
+  std_dev = sum/double(count);
+  std_dev = std_dev.sqrt();


It probably makes sense to move this to a standalone function, since other features might use this (and so you don't have to copy it for the 3D case below). It would be nice to verify that this method also works as expected in a unit test since it just came from a paper we found online

marip8 · 2020-06-17T21:47:38Z

rct_optimizations/src/validation/noise_qualifier.cpp

+  output.y.std_dev = sqrt(boost::accumulators::variance(y_acc));
+  output.z.std_dev = sqrt(boost::accumulators::variance(z_acc));
+
+  //Mean quaternion is found using method described by Markley et al: Quaternion Averaging


Add the website for future reference (or the PDF to the docs folder)

marip8 · 2020-06-17T21:48:12Z

rct_optimizations/include/rct_optimizations/validation/noise_qualifier.h

+   NoiseStatistics i;
+   NoiseStatistics j;
+   NoiseStatistics k;
+   NoiseStatistics w;


Please name these qx, qy, qz, and qw to conform to normal conventions

marip8 · 2020-06-17T21:57:13Z

rct_optimizations/include/rct_optimizations/validation/noise_qualifier.h

+ /**
+  * @brief qualifyNoise2D This function qualifies 2d sensor noise by
+  * comparing PnP results from images taken with the same poses.
+  * Sensor noise can be understood by inspecting the returned standard
+  * deviations. Position and orientation is returned, but orientation
+  * is expressed in Euler angles and may not be consistent for
+  * similar positions.
+  * @param Sets of PnP 2D problem parameters
+  * @return Noise Statistics: a vector of means & std devs
+  */
+PnPNoiseStat qualifyNoise2D(const std::vector<PnPProblem>& params);
+
+ /**
+  * @brief qualifyNoise3D This function qualifies 3d sensor noise by
+  * comparing PnP results from scans taken with the same poses.
+  * Sensor noise can be understood by inspecting the returned standard
+  * deviations. Position and orientation is returned, but orientation
+  * is expressed in Euler angles and may not be consistent for
+  * similar positions.
+  * @param params 3D image parameters
+  * @return Noise Statiscics: a vector of standard deviations and the mean pos
+  */


Update documentation references to Euler angles

marip8 · 2020-06-17T22:05:29Z

rct_optimizations/test/noise_qualification_utest.cpp

+  Eigen::Quaterniond q_ver;
+  Eigen::Matrix3d m = camera_loc.rotation();
+  q_ver = m;


Can construct the quaternion inline

marip8 · 2020-06-17T22:08:56Z

rct_optimizations/test/noise_qualification_utest.cpp

+  EXPECT_TRUE(output.x.std_dev < 1.0e-14);
+  EXPECT_TRUE(output.y.std_dev < 1.0e-14);
+  EXPECT_TRUE(output.z.std_dev < 1.0e-14);
+  EXPECT_TRUE(output.i.std_dev < 1.0e-14);
+  EXPECT_TRUE(output.j.std_dev < 1.0e-14);
+  EXPECT_TRUE(output.k.std_dev < 1.0e-14);
+  EXPECT_TRUE(output.w.std_dev < 1.0e-14);
+
+  //Absolute value of quaternion is taken, since quaternions equal their oppoisite
+  EXPECT_TRUE(abs(output.x.mean - camera_loc.translation()(0)) < 1.0e-14);
+  EXPECT_TRUE(abs(output.y.mean - camera_loc.translation()(1)) < 1.0e-14);
+  EXPECT_TRUE(abs(output.z.mean - camera_loc.translation()(2)) < 1.0e-14);
+  EXPECT_TRUE(abs(output.i.mean) - abs(q_ver.x()) < 1.0e-14);
+  EXPECT_TRUE(abs(output.j.mean) - abs(q_ver.y()) < 1.0e-14);
+  EXPECT_TRUE(abs(output.k.mean) - abs(q_ver.z()) < 1.0e-14);
+  EXPECT_TRUE(abs(output.w.mean) - abs(q_ver.w()) < 1.0e-14);


You might consider using EXPECT_LT instead. If/when this fails, EXPECT_TRUE will only tell you the condition was not right, but EXPECT_LT will print the actual and expected values to the terminal which is helpful for debug

marip8 · 2020-06-17T22:11:17Z

rct_optimizations/test/noise_qualification_utest.cpp

+
+  //add noise boilerplate
+  const double mean = 0.0;
+  const double stddev = 0.001;


This standard deviation is in pixels (as you are applying it to the image u, v coordinates), and seems really small. You might make it something more reasonable like 1.0 pixel

marip8 · 2020-06-17T23:24:52Z

rct_optimizations/test/noise_qualification_utest.cpp

+  EXPECT_TRUE(output.x.std_dev < 1.5 * stddev);
+  EXPECT_TRUE(output.y.std_dev < 1.5 * stddev);
+  EXPECT_TRUE(output.z.std_dev < 1.5 * stddev);
+  EXPECT_TRUE(output.i.std_dev < 1.5 * stddev);
+  EXPECT_TRUE(output.j.std_dev < 1.5 * stddev);
+  EXPECT_TRUE(output.k.std_dev < 1.5 * stddev);
+  EXPECT_TRUE(output.w.std_dev < 1.5 * stddev);
+
+  //Absolute value of quaternion is taken, since quaternions equal their oppoisite
+  EXPECT_TRUE(abs(output.x.mean - camera_loc.translation()(0)) < 1.5 * stddev);
+  EXPECT_TRUE(abs(output.y.mean - camera_loc.translation()(1)) < 1.5 * stddev);
+  EXPECT_TRUE(abs(output.z.mean - camera_loc.translation()(2)) < 1.5 * stddev);
+  EXPECT_TRUE(abs(output.i.mean) - abs(q_ver.x()) < 1.5 * stddev);
+  EXPECT_TRUE(abs(output.j.mean) - abs(q_ver.y()) < 1.5 * stddev);
+  EXPECT_TRUE(abs(output.k.mean) - abs(q_ver.z()) < 1.5 * stddev);
+  EXPECT_TRUE(abs(output.w.mean) - abs(q_ver.w()) < 1.5 * stddev);


I think we can compare position a little bit more accurately. Using the camera projection matrix we can come up with the differentials of (x, y, z) with respect to our perturbed input (u, v)

delta_x < (z / f_x) * delta_u

dx < (camera_loc.translation.z() / camera.fx()) * stddev

delta_y < (z / f_y) * delta_u

dy < (camera_loc.translation.z() / camera.fy()) * stddev

delta_z < (f_x * x * (u - c_x)^-2) * delta_u

dz < (camera.fx() * camera_loc.translation.x() * std::pow((u - camera.cx()), -2.0)) * stddev

u in this case is obtained by projecting the actual point into the camera

As far as orientation, I don't think we can directly compare it to the pixel noise level (i.e. stddev), so I would suggest setting a somewhat arbitrary value as the threshold and comparing distance between the mean and expected quaternion with:

EXPECT_LT(Eigen::Quaterniond(output.w.mean, output.i.mean, output.j.mean, output.k.mean).angularDistance(q_ver), angular_threshold);

marip8 · 2020-06-17T23:32:52Z

rct_optimizations/test/pnp_utest.cpp

+  PnPResult res = rct_optimizations::optimize(setup);
+
+  EXPECT_TRUE(res.converged);
+  //costs are oddly high for 3d, perfect guess


This would be an indication that your cost function is not correct. If you put in a perfect initial guess and there is no simulated noise in your correspondences, your initial cost per observation should be close to the machine epsilon. You should be seeing error on the order of 1.0e-15.

Another way to validate is to make a test where the starting condition is not perfect and see if it converges with low residual error

…ented, and some improved methods

…ther documentation

…NaN returns or 60 deg oritentation shits

Sensor Noise Qualification Updates

colin-lewis-19 force-pushed the update/sensor_noise_qual branch 2 times, most recently from 636ddf5 to f433458 Compare June 4, 2020 21:37

schornakj reviewed Jun 5, 2020

View reviewed changes

rct_optimizations/CMakeLists.txt Outdated Show resolved Hide resolved

marip8 requested changes Jun 8, 2020

View reviewed changes

colin-lewis-19 mentioned this pull request Jun 10, 2020

WIP: Update/proclivity #56

Closed

marip8 requested changes Jun 10, 2020

View reviewed changes

colin-lewis-19 force-pushed the update/sensor_noise_qual branch 4 times, most recently from 8804cf3 to 3162925 Compare June 16, 2020 18:36

marip8 requested changes Jun 17, 2020

View reviewed changes

ctlewis added 18 commits June 25, 2020 10:10

pnp3d builds

94e889a

changed derpicated struct member

a052bf5

preliminary library WIP commit

3a79b11

executable outline

a1e55fd

pnp unit test passes

9d9970d

fixed issues form stash

a243600

moving from image_tools to optimizations

fe0c2a1

Builds with main file in rct_examples, and lib in rct_optimization.

f3488dc

trying to test unit test

ff73d54

Minimal viable for 2d, but needs significant code cleaning, 3d implem…

d10a476

…ented, and some improved methods

preliminary PR build

2e0f5b8

PR revision pt.1. Missing gaussian noise, stat struct rework, and fur…

7e4259b

…ther documentation

Changed test pose

84b99f7

more documentation

9124af6

debugging pnp

5920b4f

angle-axis representation; still fails. Switching to quaternions

12253ab

quaternion tests still fail

1beddf2

Quaternion Method Functioning

7eb48a3

ctlewis added 5 commits June 25, 2020 10:40

squash when things work. Temp commit: 3d pnp is very inaccurate

a3ee564

3d noise qualification fails. Occasional innacuracy, with occasional …

6a3f2c9

…NaN returns or 60 deg oritentation shits

squash me; commiting for rebase

6c9bd74

local paramterization may have solved 3d accuracy

c801536

Removed debug prints, added pnp 3d noise test

39f8d42

colin-lewis-19 force-pushed the update/sensor_noise_qual branch from 3162925 to 39f8d42 Compare June 25, 2020 18:40

ctlewis and others added 10 commits June 25, 2020 14:04

raised angular tolerance to 8 degrees, for xenial compatability

213395e

removed commented code

d347567

Revised noise qualification code

0596eac

Updated noise qualification unit test

c2ff069

Renamed to noise qualification

290a5e5

Revised noise qualification example

37d3d7f

Moved and renamed noise qualification tool

cba06b1

Added example launch file for noise qualification

158fbde

Merge pull request #1 from marip8/update/sensor_noise_qual

096ddeb

Sensor Noise Qualification Updates

raised quaternion sampling

ee2ab5a

marip8 merged commit 1273472 into Jmeyer1292:master Jun 29, 2020

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Update/sensor noise qual #52

Update/sensor noise qual #52

colin-lewis-19 commented Jun 4, 2020

marip8 commented Jun 8, 2020 •

edited

Loading

marip8 Jun 17, 2020

marip8 Jun 17, 2020

marip8 Jun 17, 2020

marip8 Jun 17, 2020

marip8 Jun 17, 2020

marip8 Jun 17, 2020

marip8 Jun 17, 2020

marip8 Jun 17, 2020

marip8 Jun 17, 2020

marip8 Jun 17, 2020 •

edited

Loading

Update/sensor noise qual #52

Update/sensor noise qual #52

Conversation

colin-lewis-19 commented Jun 4, 2020

marip8 commented Jun 8, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

marip8 Jun 17, 2020 • edited Loading

Choose a reason for hiding this comment

marip8 commented Jun 8, 2020 •

edited

Loading

marip8 Jun 17, 2020 •

edited

Loading