SmartPoseProjFactor error with sensor offset.

[Nikolaus Mitchell]

This could be because I understand the SmartFactors incorrectly. If so sorry and just close this issue.

I noticed that the SmartProjectionPoseFactor outputs a different error value depending on whether the body_p_sensor value is set. I believe this is incorrect.

As I understand it the smartfactor triangulates the points using the current pose values and the measurements. The error is then the reprojection error using those triangulated points. Now if we add a sensor offset we change the locations of the cameras and the triangulated 3D points, however, the relationship between the Camera reprojections and the 3D points should still be the same. Thus no matter the sensor offset the reprojection error should still be the same.

Test code is below. Base is from gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

It fails with "expected 59573.902570012251 but was: 72258.758717116463"

TEST( SmartProjectionPoseFactor, SameError ) {

  using namespace vanillaPose;

  vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK));
  smartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK));
  smartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK));
  smartFactor3->add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
  graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
                           Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  // initialize third pose with some noise, we expect it to move back to original pose_above
  values.insert(x3, pose_above * noise_pose);
  double err1=graph.error(values);

//Now with sensor offset
  gtsam::Vector3 offset;
  offset << 0,0,.75;
  gtsam::Pose3 cam_offset(gtsam::Rot3(),offset);

  SmartFactor::shared_ptr dsmartFactor1(new SmartFactor(model, sharedK,cam_offset));
  dsmartFactor1->add(measurements_cam1, views);

  SmartFactor::shared_ptr dsmartFactor2(new SmartFactor(model, sharedK,cam_offset));
  dsmartFactor2->add(measurements_cam2, views);

  SmartFactor::shared_ptr dsmartFactor3(new SmartFactor(model, sharedK,cam_offset));
  dsmartFactor3->add(measurements_cam3, views);


  NonlinearFactorGraph dgraph;
  dgraph.push_back(dsmartFactor1);
  dgraph.push_back(dsmartFactor2);
  dgraph.push_back(dsmartFactor3);
  dgraph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
  dgraph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);

  double err2=dgraph.error(values);

  EXPECT_DOUBLES_EQUAL(err1,err2,1e-8);


}

[Frank Dellaert]

Thanks. If this is true we should definitely fix it. Could you try and pare down the test even more to a single smart factor?

[Nikolaus Mitchell]

Here is a version with just one smartfactor added to both graphs.

TEST( SmartProjectionPoseFactor, SameError ) {

  using namespace vanillaPose;

  vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK));
  smartFactor1->add(measurements_cam1, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
  graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
                           Point3(0.1, 0.1, 0.1)); // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  // initialize third pose with some noise, we expect it to move back to original pose_above
  values.insert(x3, pose_above * noise_pose);
  double err1=graph.error(values);

  gtsam::Vector3 offset;
  offset << 0,0,.75;
  gtsam::Pose3 cam_offset(gtsam::Rot3(),offset);

  SmartFactor::shared_ptr offset_smartFactor1(new SmartFactor(model, sharedK,cam_offset));
  offset_smartFactor1->add(measurements_cam1, views);


  NonlinearFactorGraph offset_graph;
  offset_graph.push_back(offset_smartFactor1);

  offset_graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
  offset_graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);

  double err_offset=offset_graph.error(values);

  EXPECT_DOUBLES_EQUAL(err1,err_offset,1e-8);


}

[Frank Dellaert]

Ah, many thanks, but that's not what I mean. I think there is a crisp test on SmartFactor itself that would allow us to be sure, which we can then add to the the testSmartFactor unit. At the moment, things are a bit indirect as they go via a factor graph. If you don't have time to do this, just let me know - it would be easier to see exactly what's going on, is all.

[Nikolaus Mitchell]

Oh sorry.

I can take a look at it, and maybe the actual error cause of the error at the end of the week.

[Frank Dellaert]

ping :-)

[Frank Dellaert]

No unit test (yet)