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

added test which optimize smartStereoFactor with missing measurements (uR)

release/4.3a0
Luca 2016-07-24 19:07:00 -04:00
parent b90e224f59
commit 6c163b0a4d
2 changed files with 74 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/slam/SmartFactorBase.h
View File

@ -230,7 +230,6 @@ public:
Z z3 = measured_.at(i);
if(isnan(z3.vector()[1])){ // .. and the right pixel is invalid
// delete influence of right point on jacobian Fs
std::cout << "unwhitenedError:: isnan(z3->uR()" << z3.vector() << std::endl;
MatrixZD& Fi = Fs->at(i);
for(size_t ii=0; ii<Dim; ii++)
Fi(1,ii) = 0.0;

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

@ -60,6 +60,8 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
static double missing_uR = std::numeric_limits<double>::quiet_NaN();
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
@ -167,7 +169,6 @@ TEST( SmartProjectionPoseFactor, noiselessWithMissingMeasurements ) {
// 1. Project two landmarks into two cameras and triangulate
StereoPoint2 level_uv = level_camera.project(landmark);
StereoPoint2 level_uv_right = level_camera_right.project(landmark);
double missing_uR = std::numeric_limits<double>::quiet_NaN();
StereoPoint2 level_uv_right_missing(level_uv_right.uL(),missing_uR,level_uv_right.v());
Values values;
@ -462,6 +463,78 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// 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, K);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
StereoCamera cam2(pose2, K);
// create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
StereoCamera cam3(pose3, K);
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(3, 0, 3.0);
// 1. Project three landmarks into three cameras and triangulate
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
// DELETE SOME MEASUREMENTS
StereoPoint2 sp = measurements_cam1[1];
measurements_cam1[1] = StereoPoint2(sp.uL(), missing_uR, sp.v());
sp = measurements_cam2[2];
measurements_cam2[2] = StereoPoint2(sp.uL(), missing_uR, sp.v());
SmartStereoProjectionParams params;
params.setLinearizationMode(JACOBIAN_SVD);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, 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, pose1);
values.insert(x2, pose2);
values.insert(x3, pose3 * noise_pose);
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3, result.at<Pose3>(x3),1e-7));
}
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {