12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

great simplification in stereo triangulation: converting stereo into a set of monocular cameras, then proceed as in the monocular case

release/4.3a0
Luca 2016-07-24 16:11:07 -04:00
parent 34c6527558
commit 3c15ef5d1e
3 changed files with 27 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -155,6 +155,11 @@ public:
/// Vector of cameras
typedef CameraSet<StereoCamera> Cameras;
/// Vector of monocular cameras (stereo treated as 2 monocular)
typedef PinholeCamera<Cal3_S2> MonoCamera;
typedef CameraSet<MonoCamera> MonoCameras;
typedef std::vector<Point2> MonoMeasurements;
/**
* Constructor
* @param params internal parameters of the smart factors
@ -240,75 +245,25 @@ public:
size_t m = cameras.size();
bool retriangulate = decideIfTriangulate(cameras);
// if(!retriangulate)
// std::cout << "retriangulate = false" << std::endl;
//
// bool retriangulate = true;
if (retriangulate) {
// std::cout << "Retriangulate " << std::endl;
std::vector<Point3> reprojections;
reprojections.reserve(m);
for(size_t i = 0; i < m; i++) {
reprojections.push_back(cameras[i].backproject(measured_[i]));
}
Point3 pw_sum(0,0,0);
for(const Point3& pw: reprojections) {
pw_sum = pw_sum + pw;
}
// average reprojected landmark
Point3 pw_avg = pw_sum / double(m);
double totalReprojError = 0;
// check if it lies in front of all cameras
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) {
result_ = TriangulationResult::BehindCamera();
return result_;
}
// check landmark distance
if (params_.triangulation.landmarkDistanceThreshold > 0 &&
pl.norm() > params_.triangulation.landmarkDistanceThreshold) {
result_ = TriangulationResult::Degenerate();
return result_;
}
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) {
const StereoPoint2& zi = measured_[i];
StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi);
totalReprojError += reprojectionError.vector().norm();
}
} // for
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) {
result_ = TriangulationResult::Degenerate();
return result_;
}
if(params_.triangulation.enableEPI) {
try {
pw_avg = triangulateNonlinear(cameras, measured_, pw_avg);
} catch(StereoCheiralityException& e) {
if(params_.verboseCheirality)
std::cout << "Cheirality Exception in SmartStereoProjectionFactor" << std::endl;
if(params_.throwCheirality)
throw;
result_ = TriangulationResult::BehindCamera();
return TriangulationResult::BehindCamera();
}
}
result_ = TriangulationResult(pw_avg);
} // if retriangulate
MonoCameras monoCameras;
MonoMeasurements monoMeasured;
for(size_t i = 0; i < m; i++) {
Pose3 leftPose = cameras[i].pose();
Cal3_S2 monoCal = cameras[i].calibration().calibration();
MonoCamera leftCamera_i(leftPose,monoCal);
Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
Pose3 rightPose = leftPose.compose( left_Pose_right );
MonoCamera rightCamera_i(rightPose,monoCal);
const StereoPoint2 zi = measured_[i];
monoCameras.push_back(leftCamera_i);
monoMeasured.push_back(Point2(zi.uL(),zi.v()));
monoCameras.push_back(rightCamera_i);
monoMeasured.push_back(Point2(zi.uR(),zi.v()));
}
if (retriangulate)
result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,
params_.triangulation);
return result_;
}
/// triangulate

2
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -102,7 +102,7 @@ public:
/**
* Variant of the previous one in which we include a set of measurements with the same noise and calibration
* @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param K the (known) camera calibration (same for all measurements)
*/

8
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -248,8 +248,6 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
@ -273,7 +271,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
// cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7);
EXPECT_DOUBLES_EQUAL(833953.92789459578, graph.error(values), 1e-7); // initial error
// get triangulated landmarks from smart factors
Point3 landmark1_smart = *smartFactor1->point();
@ -335,7 +333,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality));
// cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl;
EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-7);
EXPECT_DOUBLES_EQUAL(833953.92789459578, graph2.error(values), 1e-7);
LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params);
Values result2 = optimizer2.optimize();
@ -562,7 +560,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9);
// dynamic outlier rejection is off
EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9);
EXPECT_DOUBLES_EQUAL(6147.3947317473921, smartFactor4b->error(values), 1e-9);
// Factors 1-3 should have valid point, factor 4 should not
EXPECT(smartFactor1->point());