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

Fixed and cleaned up unit test

release/4.3a0
cbeall3 2015-08-19 00:11:35 -04:00
parent 1279038c77
commit 1727b60728
2 changed files with 93 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -24,6 +24,7 @@
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
@ -233,10 +234,6 @@ 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();
// }
bool retriangulate = decideIfTriangulate(cameras);
// if(!retriangulate)
@ -245,7 +242,7 @@ public:
// 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++) {
@ -259,21 +256,22 @@ public:
// average reprojected landmark
Point3 pw_avg = pw_sum / double(m);
// check if it lies in front of all cameras
bool cheirality_ok = true;
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) {
cheirality_ok = false;
break;
result_ = TriangulationResult::BehindCamera();
return result_;
}
// check landmark distance
if (params_.triangulation.landmarkDistanceThreshold > 0 &&
pl.norm() > params_.triangulation.landmarkDistanceThreshold) {
return TriangulationResult::Degenerate();
result_ = TriangulationResult::Degenerate();
return result_;
}
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) {
@ -284,20 +282,27 @@ public:
} // for
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold)
return TriangulationResult::Degenerate();
if(cheirality_ok == false) {
result_ = TriangulationResult::BehindCamera();
&& totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) {
result_ = TriangulationResult::Degenerate();
return result_;
}
if(params_.triangulation.enableEPI)
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
return result_;
}
@ -547,7 +552,6 @@ public:
//
// return Base::totalReprojectionError(cameras, backprojected);
} else {
std::cout << "Degenerate factor" << std::endl;
// if we don't want to manage the exceptions we discard the factor
return 0.0;
}

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

@ -208,6 +208,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K2);
@ -226,11 +227,11 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
Point3 landmark3(3, 0, 3.0);
// 1. Project three landmarks into three cameras and triangulate
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
vector<Key> views;
@ -238,17 +239,21 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
views.push_back(x2);
views.push_back(x3);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor());
smartFactor1->add(measurements_cam1, views, model, K2);
SmartStereoProjectionParams smart_params;
smart_params.triangulation.enableEPI = true;
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(smart_params));
smartFactor1->add(measurements_l1, views, model, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor());
smartFactor2->add(measurements_cam2, views, model, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(smart_params));
smartFactor2->add(measurements_l2, views, model, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor());
smartFactor3->add(measurements_cam3, views, model, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(smart_params));
smartFactor3->add(measurements_l3, views, model, K2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
@ -271,7 +276,13 @@ 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(979345.4, graph.error(values), 1);
// cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-9);
// get triangulated landmarks from smart factors
Point3 landmark1_smart = *smartFactor1->point();
Point3 landmark2_smart = *smartFactor2->point();
Point3 landmark3_smart = *smartFactor3->point();
Values result;
gttic_(SmartStereoProjectionPoseFactor);
@ -282,6 +293,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
// cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl;
GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
VectorValues delta = GFG->optimize();
VectorValues expected = VectorValues::Zero(delta);
@ -289,6 +302,51 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
// result.print("results of 3 camera, 3 landmark optimization \n");
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
/* ***************************************************************
* Same problem with regular Stereo factors, expect same error!
* ****************************************************************/
// landmark1_smart.print("landmark1_smart");
// landmark2_smart.print("landmark2_smart");
// landmark3_smart.print("landmark3_smart");
// add landmarks to values
values.insert(L(1), landmark1_smart);
values.insert(L(2), landmark2_smart);
values.insert(L(3), landmark3_smart);
// add factors
NonlinearFactorGraph graph2;
graph2.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph2.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
typedef GenericStereoFactor<Pose3, Point3> ProjectionFactor;
bool verboseCheirality = true;
graph2.push_back(ProjectionFactor(measurements_l1[0], model, x1, L(1), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l1[1], model, x2, L(1), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l1[2], model, x3, L(1), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l2[0], model, x1, L(2), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l2[1], model, x2, L(2), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l2[2], model, x3, L(2), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l3[0], model, x1, L(3), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l3[1], model, x2, L(3), K2, false, verboseCheirality));
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-9);
LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params);
Values result2 = optimizer2.optimize();
EXPECT_DOUBLES_EQUAL(0, graph2.error(result2), 1e-5);
// cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl;
}
/* *************************************************************************/
@ -508,7 +566,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9);
// dynamic outlier rejection is off
EXPECT_DOUBLES_EQUAL(6272.613220592455, smartFactor4b->error(values), 1e-9);
EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9);
// Factors 1-3 should have valid point, factor 4 should not
EXPECT(smartFactor1->point());