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

fixing tests

release/4.3a0
lcarlone 2021-10-02 21:47:44 -04:00
parent 4c10be9808
commit 117f0d1f45
3 changed files with 301 additions and 294 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -64,7 +64,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
typename Base::Cameras cameraRig_; typename Base::Cameras cameraRig_;
/// vector of camera Ids (one for each observation), identifying which camera took the measurement /// vector of camera Ids (one for each observation), identifying which camera took the measurement
KeyVector cameraIds_; FastVector<size_t> cameraIds_;
public: public:
typedef CAMERA Camera; typedef CAMERA Camera;
@ -145,7 +145,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
} }
/// return the calibration object /// return the calibration object
inline KeyVector cameraIds() const { inline FastVector<size_t> cameraIds() const {
return cameraIds_; return cameraIds_;
} }
@ -184,11 +184,12 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras(const Values& values) const override {
typename Base::Cameras cameras; typename Base::Cameras cameras;
for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
const Pose3& body_P_cam_i = cameraRig_[i].pose(); const Key cameraId = cameraIds_[i];
const Pose3& body_P_cam_i = cameraRig_[cameraId].pose();
const Pose3 world_P_sensor_i = values.at<Pose3>(nonUniqueKeys_[i]) const Pose3 world_P_sensor_i = values.at<Pose3>(nonUniqueKeys_[i])
* body_P_cam_i; * body_P_cam_i;
cameras.emplace_back(world_P_sensor_i, cameras.emplace_back(world_P_sensor_i,
make_shared<typename CAMERA::CalibrationType>(cameraRig_[i].calibration())); make_shared<typename CAMERA::CalibrationType>(cameraRig_[cameraId].calibration()));
} }
return cameras; return cameras;
} }
@ -220,7 +221,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
} else { // valid result: compute jacobians } else { // valid result: compute jacobians
b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
for (size_t i = 0; i < Fs.size(); i++) { for (size_t i = 0; i < Fs.size(); i++) {
const Pose3 body_P_sensor = cameraRig_[i].pose(); const Key cameraId = cameraIds_[i];
const Pose3 body_P_sensor = cameraRig_[cameraId].pose();
const Pose3 sensor_P_body = body_P_sensor.inverse(); const Pose3 sensor_P_body = body_P_sensor.inverse();
const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; const Pose3 world_P_body = cameras[i].pose() * sensor_P_body;
Eigen::Matrix<double, DimPose, DimPose> H; Eigen::Matrix<double, DimPose, DimPose> H;
@ -242,7 +244,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
// Create structures for Hessian Factors // Create structures for Hessian Factors
KeyVector js; FastVector<size_t> js;
FastVector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); FastVector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
FastVector < Vector > gs(nrUniqueKeys); FastVector < Vector > gs(nrUniqueKeys);

1
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -84,6 +84,7 @@ Camera cam3(pose_above, sharedK);
// default Cal3_S2 poses // default Cal3_S2 poses
namespace vanillaPose2 { namespace vanillaPose2 {
typedef PinholePose<Cal3_S2> Camera; typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor; typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
typedef SmartProjectionRigFactor<Camera> SmartFactorP; typedef SmartProjectionRigFactor<Camera> SmartFactorP;
static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));

580
gtsam/slam/tests/testSmartProjectionRigFactor.cpp
View File

@ -45,6 +45,10 @@ static Symbol x1('X', 1);
static Symbol x2('X', 2); static Symbol x2('X', 2);
static Symbol x3('X', 3); static Symbol x3('X', 3);
Key cameraId1 = 0; // first camera
Key cameraId2 = 1;
Key cameraId3 = 2;
static Point2 measurement1(323.0, 240.0); static Point2 measurement1(323.0, 240.0);
LevenbergMarquardtParams lmParams; LevenbergMarquardtParams lmParams;
@ -59,294 +63,294 @@ TEST( SmartProjectionRigFactor, Constructor) {
SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig));
} }
///* ************************************************************************* */ /* ************************************************************************* */
//TEST( SmartProjectionRigFactor, Constructor2) { TEST( SmartProjectionRigFactor, Constructor2) {
// using namespace vanillaPose; using namespace vanillaPose;
// SmartProjectionParams params; Cameras cameraRig;
// params.setRankTolerance(rankTol); SmartProjectionParams params;
// SmartFactorP factor1(model, params); params.setRankTolerance(rankTol);
//} SmartFactorP factor1(model, cameraRig, params);
// }
///* ************************************************************************* */
//TEST( SmartProjectionRigFactor, Constructor3) { /* ************************************************************************* */
// using namespace vanillaPose; TEST( SmartProjectionRigFactor, Constructor3) {
// SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); using namespace vanillaPose;
// factor1->add(measurement1, x1, sharedK); Cameras cameraRig;
//} cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
// SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig));
///* ************************************************************************* */ factor1->add(measurement1, x1, cameraId1);
//TEST( SmartProjectionRigFactor, Constructor4) { }
// using namespace vanillaPose;
// SmartProjectionParams params; /* ************************************************************************* */
// params.setRankTolerance(rankTol); TEST( SmartProjectionRigFactor, Constructor4) {
// SmartFactorP factor1(model, params); using namespace vanillaPose;
// factor1.add(measurement1, x1, sharedK); Cameras cameraRig;
//} cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
// SmartProjectionParams params;
///* ************************************************************************* */ params.setRankTolerance(rankTol);
//TEST( SmartProjectionRigFactor, Equals ) { SmartFactorP factor1(model, cameraRig, params);
// using namespace vanillaPose; factor1.add(measurement1, x1, cameraId1);
// SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); }
// factor1->add(measurement1, x1, sharedK);
// /* ************************************************************************* */
// SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); TEST( SmartProjectionRigFactor, Equals ) {
// factor2->add(measurement1, x1, sharedK); using namespace vanillaPose;
// Cameras cameraRig; // single camera in the rig
// CHECK(assert_equal(*factor1, *factor2)); cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
//}
// SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig));
///* *************************************************************************/ factor1->add(measurement1, x1, cameraId1);
//TEST( SmartProjectionRigFactor, noiseless ) {
// SmartFactorP::shared_ptr factor2(new SmartFactorP(model, cameraRig));
// using namespace vanillaPose; factor2->add(measurement1, x1, cameraId1);
//
// // Project two landmarks into two cameras CHECK(assert_equal(*factor1, *factor2));
// Point2 level_uv = level_camera.project(landmark1); }
// Point2 level_uv_right = level_camera_right.project(landmark1);
// /* *************************************************************************/
// SmartFactorP factor(model); TEST( SmartProjectionRigFactor, noiseless ) {
// factor.add(level_uv, x1, sharedK);
// factor.add(level_uv_right, x2, sharedK); using namespace vanillaPose;
//
// Values values; // it's a pose factor, hence these are poses Cameras cameraRig; // single camera in the rig
// values.insert(x1, cam1.pose()); cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
// values.insert(x2, cam2.pose());
// // Project two landmarks into two cameras
// double actualError = factor.error(values); Point2 level_uv = level_camera.project(landmark1);
// double expectedError = 0.0; Point2 level_uv_right = level_camera_right.project(landmark1);
// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
// SmartFactorP factor(model, cameraRig);
// SmartFactorP::Cameras cameras = factor.cameras(values); factor.add(level_uv, x1, cameraId1); // both taken from the same camera
// double actualError2 = factor.totalReprojectionError(cameras); factor.add(level_uv_right, x2, cameraId1);
// EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
// Values values; // it's a pose factor, hence these are poses
// // Calculate expected derivative for point (easiest to check) values.insert(x1, cam1.pose());
// std::function<Vector(Point3)> f = // values.insert(x2, cam2.pose());
// std::bind(&SmartFactorP::whitenedError<Point3>, factor, cameras,
// std::placeholders::_1); double actualError = factor.error(values);
// double expectedError = 0.0;
// // Calculate using computeEP EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
// Matrix actualE;
// factor.triangulateAndComputeE(actualE, values); SmartFactorP::Cameras cameras = factor.cameras(values);
// double actualError2 = factor.totalReprojectionError(cameras);
// // get point EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
// boost::optional<Point3> point = factor.point();
// CHECK(point); // Calculate expected derivative for point (easiest to check)
// std::function<Vector(Point3)> f = //
// // calculate numerical derivative with triangulated point std::bind(&SmartFactorP::whitenedError<Point3>, factor, cameras,
// Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point); std::placeholders::_1);
// EXPECT(assert_equal(expectedE, actualE, 1e-7));
// // Calculate using computeEP
// // Calculate using reprojectionError Matrix actualE;
// SmartFactorP::Cameras::FBlocks F; factor.triangulateAndComputeE(actualE, values);
// Matrix E;
// Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); // get point
// EXPECT(assert_equal(expectedE, E, 1e-7)); boost::optional<Point3> point = factor.point();
// CHECK(point);
// EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7));
// // calculate numerical derivative with triangulated point
// // Calculate using computeJacobians Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point);
// Vector b; EXPECT(assert_equal(expectedE, actualE, 1e-7));
// SmartFactorP::FBlocks Fs;
// factor.computeJacobians(Fs, E, b, cameras, *point); // Calculate using reprojectionError
// double actualError3 = b.squaredNorm(); SmartFactorP::Cameras::FBlocks F;
// EXPECT(assert_equal(expectedE, E, 1e-7)); Matrix E;
// EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
//} EXPECT(assert_equal(expectedE, E, 1e-7));
//
///* *************************************************************************/ EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7));
//TEST( SmartProjectionRigFactor, noisy ) {
// // Calculate using computeJacobians
// using namespace vanillaPose; Vector b;
// SmartFactorP::FBlocks Fs;
// // Project two landmarks into two cameras factor.computeJacobians(Fs, E, b, cameras, *point);
// Point2 pixelError(0.2, 0.2); double actualError3 = b.squaredNorm();
// Point2 level_uv = level_camera.project(landmark1) + pixelError; EXPECT(assert_equal(expectedE, E, 1e-7));
// Point2 level_uv_right = level_camera_right.project(landmark1); EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
// }
// Values values;
// values.insert(x1, cam1.pose()); /* *************************************************************************/
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), TEST( SmartProjectionRigFactor, noisy ) {
// Point3(0.5, 0.1, 0.3));
// values.insert(x2, pose_right.compose(noise_pose)); using namespace vanillaPose;
//
// SmartFactorP::shared_ptr factor(new SmartFactorP(model)); Cameras cameraRig; // single camera in the rig
// factor->add(level_uv, x1, sharedK); cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
// factor->add(level_uv_right, x2, sharedK);
// // Project two landmarks into two cameras
// double actualError1 = factor->error(values); Point2 pixelError(0.2, 0.2);
// Point2 level_uv = level_camera.project(landmark1) + pixelError;
// SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); Point2 level_uv_right = level_camera_right.project(landmark1);
// Point2Vector measurements;
// measurements.push_back(level_uv); Values values;
// measurements.push_back(level_uv_right); values.insert(x1, cam1.pose());
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; Point3(0.5, 0.1, 0.3));
// sharedKs.push_back(sharedK); values.insert(x2, pose_right.compose(noise_pose));
// sharedKs.push_back(sharedK);
// SmartFactorP::shared_ptr factor(new SmartFactorP(model,cameraRig));
// KeyVector views { x1, x2 }; factor->add(level_uv, x1, cameraId1);
// factor->add(level_uv_right, x2, cameraId1);
// factor2->add(measurements, views, sharedKs);
// double actualError2 = factor2->error(values); double actualError1 = factor->error(values);
// DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
//} // create other factor by passing multiple measurements
// SmartFactorP::shared_ptr factor2(new SmartFactorP(model,cameraRig));
///* *************************************************************************/
//TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { Point2Vector measurements;
// using namespace vanillaPose; measurements.push_back(level_uv);
// measurements.push_back(level_uv_right);
// // create arbitrary body_T_sensor (transforms from sensor to body)
// Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), KeyVector views { x1, x2 };
// Point3(1, 1, 1)); FastVector<size_t> cameraIds { 0, 0 };
//
// // These are the poses we want to estimate, from camera measurements factor2->add(measurements, views, cameraIds);
// const Pose3 sensor_T_body = body_T_sensor.inverse(); double actualError2 = factor2->error(values);
// Pose3 wTb1 = cam1.pose() * sensor_T_body; DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
// Pose3 wTb2 = cam2.pose() * sensor_T_body; }
// Pose3 wTb3 = cam3.pose() * sensor_T_body;
// /* *************************************************************************/
// // three landmarks ~5 meters infront of camera TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
// Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); using namespace vanillaPose;
//
// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // create arbitrary body_T_sensor (transforms from sensor to body)
// Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
// // Project three landmarks into three cameras Point3(1, 1, 1));
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); Cameras cameraRig; // single camera in the rig
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); cameraRig.push_back( Camera(body_T_sensor, sharedK) );
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// // These are the poses we want to estimate, from camera measurements
// // Create smart factors const Pose3 sensor_T_body = body_T_sensor.inverse();
// KeyVector views { x1, x2, x3 }; Pose3 wTb1 = cam1.pose() * sensor_T_body;
// Pose3 wTb2 = cam2.pose() * sensor_T_body;
// SmartProjectionParams params; Pose3 wTb3 = cam3.pose() * sensor_T_body;
// params.setRankTolerance(1.0);
// params.setDegeneracyMode(IGNORE_DEGENERACY); // three landmarks ~5 meters infront of camera
// params.setEnableEPI(false); Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0);
//
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// sharedKs.push_back(sharedK);
// sharedKs.push_back(sharedK); // Project three landmarks into three cameras
// sharedKs.push_back(sharedK); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// std::vector<Pose3> body_T_sensors; projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// body_T_sensors.push_back(body_T_sensor);
// body_T_sensors.push_back(body_T_sensor); // Create smart factors
// body_T_sensors.push_back(body_T_sensor); KeyVector views { x1, x2, x3 };
// FastVector<size_t> cameraIds { 0, 0, 0 };
// SmartFactorP smartFactor1(model, params);
// smartFactor1.add(measurements_cam1, views, sharedKs, body_T_sensors); SmartProjectionParams params;
// params.setRankTolerance(1.0);
// SmartFactorP smartFactor2(model, params); params.setDegeneracyMode(IGNORE_DEGENERACY);
// smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors); params.setEnableEPI(false);
//
// SmartFactorP smartFactor3(model, params); SmartFactorP smartFactor1(model, cameraRig, params);
// smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors); smartFactor1.add(measurements_cam1, views, cameraIds);
// ;
// SmartFactorP smartFactor2(model, cameraRig, params);
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); smartFactor2.add(measurements_cam2, views, cameraIds);
//
// // Put all factors in factor graph, adding priors SmartFactorP smartFactor3(model, cameraRig, params);
// NonlinearFactorGraph graph; smartFactor3.add(measurements_cam3, views, cameraIds);
// graph.push_back(smartFactor1);
// graph.push_back(smartFactor2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// graph.push_back(smartFactor3);
// graph.addPrior(x1, wTb1, noisePrior); // Put all factors in factor graph, adding priors
// graph.addPrior(x2, wTb2, noisePrior); NonlinearFactorGraph graph;
// graph.push_back(smartFactor1);
// // Check errors at ground truth poses graph.push_back(smartFactor2);
// Values gtValues; graph.push_back(smartFactor3);
// gtValues.insert(x1, wTb1); graph.addPrior(x1, wTb1, noisePrior);
// gtValues.insert(x2, wTb2); graph.addPrior(x2, wTb2, noisePrior);
// gtValues.insert(x3, wTb3);
// double actualError = graph.error(gtValues); // Check errors at ground truth poses
// double expectedError = 0.0; Values gtValues;
// DOUBLES_EQUAL(expectedError, actualError, 1e-7) gtValues.insert(x1, wTb1);
// gtValues.insert(x2, wTb2);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), gtValues.insert(x3, wTb3);
// Point3(0.1, 0.1, 0.1)); double actualError = graph.error(gtValues);
// Values values; double expectedError = 0.0;
// values.insert(x1, wTb1); DOUBLES_EQUAL(expectedError, actualError, 1e-7)
// values.insert(x2, wTb2);
// // initialize third pose with some noise, we expect it to move back to Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
// // original pose3 Point3(0.1, 0.1, 0.1));
// values.insert(x3, wTb3 * noise_pose); Values values;
// values.insert(x1, wTb1);
// LevenbergMarquardtParams lmParams; values.insert(x2, wTb2);
// Values result; // initialize third pose with some noise, we expect it to move back to
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); // original pose3
// result = optimizer.optimize(); values.insert(x3, wTb3 * noise_pose);
//// graph.print("graph\n");
// EXPECT(assert_equal(wTb3, result.at<Pose3>(x3))); LevenbergMarquardtParams lmParams;
//} Values result;
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
///* *************************************************************************/ result = optimizer.optimize();
//TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { EXPECT(assert_equal(wTb3, result.at<Pose3>(x3)));
// }
// using namespace vanillaPose2;
// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; /* *************************************************************************/
// TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) {
// // Project three landmarks into three cameras
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); using namespace vanillaPose2;
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Cameras cameraRig; // single camera in the rig
// KeyVector views; cameraRig.push_back( Camera(Pose3::identity(), sharedK2) );
// views.push_back(x1);
// views.push_back(x2); // Project three landmarks into three cameras
// views.push_back(x3); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// sharedK2s.push_back(sharedK2);
// sharedK2s.push_back(sharedK2); KeyVector views {x1,x2,x3};
// sharedK2s.push_back(sharedK2); FastVector<size_t> cameraIds{0,0,0};// 3 measurements from the same camera in the rig
//
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,cameraRig));
// smartFactor1->add(measurements_cam1, views, sharedK2s); smartFactor1->add(measurements_cam1, views, cameraIds);
//
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model,cameraRig));
// smartFactor2->add(measurements_cam2, views, sharedK2s); smartFactor2->add(measurements_cam2, views, cameraIds);
//
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model,cameraRig));
// smartFactor3->add(measurements_cam3, views, sharedK2s); smartFactor3->add(measurements_cam3, views, cameraIds);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
//
// NonlinearFactorGraph graph; NonlinearFactorGraph graph;
// graph.push_back(smartFactor1); graph.push_back(smartFactor1);
// graph.push_back(smartFactor2); graph.push_back(smartFactor2);
// graph.push_back(smartFactor3); graph.push_back(smartFactor3);
// graph.addPrior(x1, cam1.pose(), noisePrior); graph.addPrior(x1, cam1.pose(), noisePrior);
// graph.addPrior(x2, cam2.pose(), noisePrior); graph.addPrior(x2, cam2.pose(), noisePrior);
//
// Values groundTruth; Values groundTruth;
// groundTruth.insert(x1, cam1.pose()); groundTruth.insert(x1, cam1.pose());
// groundTruth.insert(x2, cam2.pose()); groundTruth.insert(x2, cam2.pose());
// groundTruth.insert(x3, cam3.pose()); groundTruth.insert(x3, cam3.pose());
// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
//
// // 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/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), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
// Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
// Values values; Values values;
// values.insert(x1, cam1.pose()); values.insert(x1, cam1.pose());
// values.insert(x2, cam2.pose()); values.insert(x2, cam2.pose());
// // initialize third pose with some noise, we expect it to move back to original pose_above // initialize third pose with some noise, we expect it to move back to original pose_above
// values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
// EXPECT( EXPECT(
// assert_equal( assert_equal(
// Pose3( Pose3(
// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
// Point3(0.1, -0.1, 1.9)), Point3(0.1, -0.1, 1.9)),
// values.at<Pose3>(x3))); values.at<Pose3>(x3)));
//
// Values result; Values result;
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
// result = optimizer.optimize(); result = optimizer.optimize();
// EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
//} }
//
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionRigFactor, Factors ) { //TEST( SmartProjectionRigFactor, Factors ) {
// //