remove monocular triangulation hack and make tests pass again
cbeall3
parent
3f0e695cc9
commit
36c652ac40
|
|
@ -234,67 +234,58 @@ public:
|
|||
TriangulationResult triangulateSafe(const Cameras& cameras) const {
|
||||
|
||||
size_t m = cameras.size();
|
||||
if (m < 2) { // if we have a single pose the corresponding factor is uninformative
|
||||
return TriangulationResult::Degenerate();
|
||||
}
|
||||
// if (m < 2) { // if we have a single pose the corresponding factor is uninformative
|
||||
// return TriangulationResult::Degenerate();
|
||||
// }
|
||||
|
||||
bool retriangulate = decideIfTriangulate(cameras);
|
||||
if (retriangulate) {
|
||||
// We triangulate the 3D position of the landmark
|
||||
std::cout << "triangulateSafe i \n" << std::endl;
|
||||
|
||||
//TODO: Chris will replace this with something else for stereo
|
||||
// point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
|
||||
// rankTolerance_, enableEPI_);
|
||||
|
||||
// // // Temporary hack to use monocular triangulation
|
||||
std::vector<Point2> mono_measurements;
|
||||
BOOST_FOREACH(const StereoPoint2& sp, this->measured_) {
|
||||
mono_measurements.push_back(sp.point2());
|
||||
std::vector<Point3> reprojections;
|
||||
reprojections.reserve(m);
|
||||
for(size_t i = 0; i < m; i++) {
|
||||
reprojections.push_back(cameras[i].backproject(measured_[i]));
|
||||
}
|
||||
|
||||
std::vector<PinholeCamera<Cal3_S2> > mono_cameras;
|
||||
BOOST_FOREACH(const StereoCamera& camera, cameras) {
|
||||
const Pose3& pose = camera.pose();
|
||||
const Cal3_S2& K = camera.calibration()->calibration();
|
||||
mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K));
|
||||
Point3 pw_sum;
|
||||
BOOST_FOREACH(const Point3& pw, reprojections) {
|
||||
pw_sum = pw_sum + pw;
|
||||
}
|
||||
// Point3 point = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
|
||||
// params_.triangulation.rankTolerance, params_.triangulation.enableEPI);
|
||||
result_ = gtsam::triangulateSafe(mono_cameras, mono_measurements,
|
||||
params_.triangulation);
|
||||
// average reprojected landmark
|
||||
Point3 pw_avg = pw_sum / double(m);
|
||||
|
||||
// // // End temporary hack
|
||||
// check if it lies in front of all cameras
|
||||
bool cheirality_ok = true;
|
||||
double totalReprojError = 0;
|
||||
for(size_t i = 0; i < m; i++) {
|
||||
const Pose3& pose = cameras[i].pose();
|
||||
const Point3& pl = pose.transform_to(pw_avg);
|
||||
if (pl.z() <= 0) {
|
||||
cheirality_ok = false;
|
||||
break;
|
||||
}
|
||||
|
||||
// FIXME: temporary: triangulation using only first camera
|
||||
// const StereoPoint2& z0 = this->measured_.at(0);
|
||||
// point_ = cameras[0].backproject(z0);
|
||||
// check landmark distance
|
||||
if (params_.triangulation.landmarkDistanceThreshold > 0 &&
|
||||
pl.norm() > params_.triangulation.landmarkDistanceThreshold) {
|
||||
return TriangulationResult::Degenerate();
|
||||
}
|
||||
|
||||
// Check landmark distance and reprojection errors to avoid outliers
|
||||
// double totalReprojError = 0.0;
|
||||
// size_t i = 0;
|
||||
// BOOST_FOREACH(const StereoCamera& camera, cameras) {
|
||||
// Point3 cameraTranslation = camera.pose().translation();
|
||||
// // we discard smart factors corresponding to points that are far away
|
||||
// if (cameraTranslation.distance(*result_) > params_.triangulation.landmarkDistanceThreshold) {
|
||||
// return TriangulationResult::Degenerate();
|
||||
// }
|
||||
// const StereoPoint2& zi = this->measured_.at(i);
|
||||
// try {
|
||||
// StereoPoint2 reprojectionError(camera.project(*result_) - zi);
|
||||
// totalReprojError += reprojectionError.vector().norm();
|
||||
// } catch (CheiralityException) {
|
||||
// return TriangulationResult::BehindCamera();
|
||||
// }
|
||||
// i += 1;
|
||||
// }
|
||||
//std::cout << "totalReprojError error: " << totalReprojError << std::endl;
|
||||
// we discard smart factors that have large reprojection error
|
||||
// if (params_.triangulation.dynamicOutlierRejectionThreshold > 0
|
||||
// && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold)
|
||||
// return TriangulationResult::Degenerate();
|
||||
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) {
|
||||
const StereoPoint2& zi = measured_[i];
|
||||
StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi);
|
||||
totalReprojError += reprojectionError.vector().norm();
|
||||
}
|
||||
} // for
|
||||
|
||||
// result_ = TriangulationResult(point);
|
||||
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0
|
||||
&& totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold)
|
||||
return TriangulationResult::Degenerate();
|
||||
|
||||
if(cheirality_ok == false) {
|
||||
result_ = TriangulationResult::BehindCamera();
|
||||
}
|
||||
result_ = TriangulationResult(pw_avg);
|
||||
|
||||
}
|
||||
return result_;
|
||||
|
|
@ -389,11 +380,11 @@ public:
|
|||
return boost::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
|
||||
}
|
||||
|
||||
/// linearize to a Hessianfactor
|
||||
virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
|
||||
const Values& values, double lambda = 0.0) const {
|
||||
return createHessianFactor(this->cameras(values), lambda);
|
||||
}
|
||||
// /// linearize to a Hessianfactor
|
||||
// virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
|
||||
// const Values& values, double lambda = 0.0) const {
|
||||
// return createHessianFactor(this->cameras(values), lambda);
|
||||
// }
|
||||
|
||||
// /// linearize to an Implicit Schur factor
|
||||
// virtual boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > linearizeToImplicit(
|
||||
|
|
@ -420,8 +411,8 @@ public:
|
|||
return createHessianFactor(cameras, lambda);
|
||||
// case IMPLICIT_SCHUR:
|
||||
// return createRegularImplicitSchurFactor(cameras, lambda);
|
||||
// case JACOBIAN_SVD:
|
||||
// return createJacobianSVDFactor(cameras, lambda);
|
||||
case JACOBIAN_SVD:
|
||||
return createJacobianSVDFactor(cameras, lambda);
|
||||
// case JACOBIAN_Q:
|
||||
// return createJacobianQFactor(cameras, lambda);
|
||||
default:
|
||||
|
|
@ -535,14 +526,11 @@ public:
|
|||
else
|
||||
result_ = triangulateSafe(cameras);
|
||||
|
||||
std::cout << "Triangulation result in totalReprojectionError" << std::endl;
|
||||
std::cout << result_ << std::endl;
|
||||
|
||||
if (result_)
|
||||
// All good, just use version in base class
|
||||
return Base::totalReprojectionError(cameras, *result_);
|
||||
else if (params_.degeneracyMode == HANDLE_INFINITY) {
|
||||
throw("Backproject at infinity");
|
||||
throw(std::runtime_error("Backproject at infinity not implemented for SmartStereo."));
|
||||
// // Otherwise, manage the exceptions with rotation-only factors
|
||||
// const StereoPoint2& z0 = this->measured_.at(0);
|
||||
// Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0);
|
||||
|
|
|
|||
|
|
@ -124,7 +124,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
|
|||
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
|
||||
Point3(0, 0, 1));
|
||||
StereoCamera level_camera(level_pose, K2);
|
||||
|
||||
cout << "Test 122 STARTS HERE ----------------------------------------- 122 " << endl;
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
|
||||
StereoCamera level_camera_right(level_pose_right, K2);
|
||||
|
|
@ -277,7 +277,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
|
|||
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
|
||||
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
|
||||
|
||||
EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1);
|
||||
EXPECT_DOUBLES_EQUAL(991819.94, graph.error(values), 1);
|
||||
|
||||
Values result;
|
||||
gttic_(SmartStereoProjectionPoseFactor);
|
||||
|
|
@ -286,7 +286,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
|
|||
gttoc_(SmartStereoProjectionPoseFactor);
|
||||
tictoc_finishedIteration_();
|
||||
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-6);
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
|
||||
|
||||
GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
|
||||
VectorValues delta = GFG->optimize();
|
||||
|
|
@ -436,7 +436,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
|
|||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
||||
|
||||
double excludeLandmarksFutherThanDist = 1e10;
|
||||
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
|
||||
|
||||
vector<Key> views;
|
||||
|
|
@ -474,7 +473,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
|
||||
SmartStereoProjectionParams params;
|
||||
params.setLinearizationMode(JACOBIAN_SVD);
|
||||
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
||||
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
|
||||
|
||||
|
||||
|
|
@ -668,11 +666,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
|
|||
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark3);
|
||||
|
||||
// Create smartfactors
|
||||
double rankTol = 10;
|
||||
|
||||
SmartStereoProjectionParams params;
|
||||
params.setRankTolerance(rankTol);
|
||||
params.setRankTolerance(10);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(params));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
|
|
@ -979,6 +974,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
|
|||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
|
||||
|
||||
vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
|
|
@ -1023,7 +1019,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
|
|||
// Hessian is invariant to rotations in the nondegenerate case
|
||||
EXPECT(
|
||||
assert_equal(hessianFactor->information(),
|
||||
hessianFactorRot->information(), 1e-8));
|
||||
hessianFactorRot->information(), 1e-7));
|
||||
|
||||
Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
|
||||
Point3(10, -4, 5));
|
||||
|
|
@ -1039,7 +1035,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
|
|||
// Hessian is invariant to rotations and translations in the nondegenerate case
|
||||
EXPECT(
|
||||
assert_equal(hessianFactor->information(),
|
||||
hessianFactorRotTran->information(), 1e-8));
|
||||
hessianFactorRotTran->information(), 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue