diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index e651244af..870f4d5db 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -58,7 +58,6 @@ private: SharedIsotropic noiseModel_; protected: - /** * 2D measurement and noise model for each of the m views * We keep a copy of measurements for I/O and computing the error. @@ -66,6 +65,10 @@ protected: */ std::vector measured_; + /// @name Pose of the camera in the body frame + const boost::optional body_P_sensor_; ///< Pose of the camera in the body frame + /// @} + static const int Dim = traits::dimension; ///< Camera dimension static const int ZDim = traits::dimension; ///< Measurement dimension @@ -105,6 +108,10 @@ public: /// We use the new CameraSte data structure to refer to a set of cameras typedef CameraSet Cameras; + /// Constructor + SmartFactorBase(boost::optional body_P_sensor = boost::none) : + body_P_sensor_(body_P_sensor){} + /// Virtual destructor, subclasses from NonlinearFactor virtual ~SmartFactorBase() { } @@ -189,6 +196,8 @@ public: std::cout << "measurement, p = " << measured_[k] << "\t"; noiseModel_->print("noise model = "); } + if(body_P_sensor_) + body_P_sensor_->print("body_P_sensor_:\n"); print("", keyFormatter); } @@ -210,7 +219,16 @@ public: Vector unwhitenedError(const Cameras& cameras, const POINT& point, boost::optional Fs = boost::none, // boost::optional E = boost::none) const { - return cameras.reprojectionError(point, measured_, Fs, E); + Vector ue = cameras.reprojectionError(point, measured_, Fs, E); + if(body_P_sensor_){ + for(size_t i=0; i < Fs->size(); i++){ + Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); + Matrix J(6, 6); + Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); + Fs->at(i) = Fs->at(i) * J; + } + } + return ue; } /** @@ -375,6 +393,14 @@ public: F.block(ZDim * i, Dim * i) = Fblocks.at(i); } + + Pose3 body_P_sensor() const{ + if(body_P_sensor_) + return *body_P_sensor_; + else + return Pose3(); // if unspecified, the transformation is the identity + } + private: /// Serialization function diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 92c9a7660..9cb9855c8 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -74,10 +74,6 @@ protected: const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) /// @} - /// @name Pose of the camera in the body frame - const boost::optional body_P_sensor_; ///< Pose of the camera in the body frame - /// @} - public: /// shorthand for a smart pointer to a factor @@ -100,15 +96,14 @@ public: double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1, boost::optional body_P_sensor = boost::none) : + Base(body_P_sensor), linearizationMode_(linearizationMode), // degeneracyMode_(degeneracyMode), // parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), // result_(TriangulationResult::Degenerate()), // retriangulationThreshold_(1e-5), // - throwCheirality_(false), verboseCheirality_(false), - body_P_sensor_(body_P_sensor){ - } + throwCheirality_(false), verboseCheirality_(false) {} /** Virtual destructor */ virtual ~SmartProjectionFactor() { @@ -125,8 +120,6 @@ public: std::cout << "linearizationMode:\n" << linearizationMode_ << std::endl; std::cout << "triangulationParameters:\n" << parameters_ << std::endl; std::cout << "result:\n" << result_ << std::endl; - if(body_P_sensor_) - body_P_sensor_->print("body_P_sensor_:\n"); Base::print("", keyFormatter); } @@ -476,13 +469,6 @@ public: return throwCheirality_; } - Pose3 body_P_sensor() const{ - if(body_P_sensor_) - return *body_P_sensor_; - else - return Pose3(); // if unspecified, the transformation is the identity - } - private: /// Serialization function diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index a540ccb52..ffd8008ec 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -270,13 +270,9 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - // result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(bodyPose3,result.at(x3))); } -#if 0 /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { @@ -308,39 +304,34 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1, noisePrior)); - graph.push_back(PriorFactor(x2, cam2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); Values groundTruth; - groundTruth.insert(x1, cam1); - groundTruth.insert(x2, cam2); - groundTruth.insert(x3, cam3); + groundTruth.insert(x1, cam1.pose()); + groundTruth.insert(x2, cam2.pose()); + groundTruth.insert(x3, cam3.pose()); 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 / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, cam1); - values.insert(x2, cam2); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, Camera(pose_above * noise_pose, sharedK2)); + values.insert(x3, pose_above * noise_pose); EXPECT( assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3).pose())); + Point3(0.1, -0.1, 1.9)), values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - -// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); -// VectorValues delta = GFG->optimize(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - EXPECT(assert_equal(pose_above, result.at(x3).pose(), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } /* *************************************************************************/ @@ -541,31 +532,29 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1, noisePrior)); - graph.push_back(PriorFactor(x2, cam2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), 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, cam1); - values.insert(x2, cam2); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, Camera(pose_above * noise_pose, sharedK)); + values.insert(x3, pose_above * noise_pose); EXPECT( assert_equal( Pose3( Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), - values.at(x3).pose())); + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - EXPECT(assert_equal(pose_above, result.at(x3).pose(), 1e-7)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } /* *************************************************************************/ @@ -603,21 +592,21 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1, noisePrior)); - graph.push_back(PriorFactor(x2, cam2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), 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, cam1); - values.insert(x2, cam2); - values.insert(x3, Camera(pose_above * noise_pose, sharedK)); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3).pose(), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } /* *************************************************************************/ @@ -660,22 +649,22 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1, noisePrior)); - graph.push_back(PriorFactor(x2, cam2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), 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, cam1); - values.insert(x2, cam2); - values.insert(x3, Camera(pose_above * noise_pose, sharedK)); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); // All factors are disabled and pose should remain where it is Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3), result.at(x3))); + EXPECT(assert_equal(values.at(x3), result.at(x3))); } /* *************************************************************************/ @@ -731,19 +720,19 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, cam1, noisePrior)); - graph.push_back(PriorFactor(x2, cam2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); Values values; - values.insert(x1, cam1); - values.insert(x2, cam2); - values.insert(x3, cam3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); // All factors are disabled and pose should remain where it is Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(cam3, result.at(x3))); + EXPECT(assert_equal(cam3.pose(), result.at(x3))); } /* *************************************************************************/ @@ -766,7 +755,6 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { SmartFactor::shared_ptr smartFactor1( new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_Q)); - smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( @@ -785,20 +773,20 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1, noisePrior)); - graph.push_back(PriorFactor(x2, cam2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); 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, cam1); - values.insert(x2, cam2); - values.insert(x3, Camera(pose_above * noise_pose, sharedK)); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3).pose(), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } /* *************************************************************************/ @@ -907,10 +895,10 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { 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, cam1); - values.insert(x2, cam2); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, Camera(pose3 * noise_pose, sharedK)); + values.insert(x3, pose3 * noise_pose); EXPECT( assert_equal( Pose3( @@ -918,7 +906,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { -0.130426831, -0.0115837907, 0.130819108, -0.98278564, -0.130455917), Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3).pose())); + values.at(x3))); boost::shared_ptr factor1 = smartFactor1->linearize(values); boost::shared_ptr factor2 = smartFactor2->linearize(values); @@ -938,14 +926,13 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { + factor2->augmentedInformation() + factor3->augmentedInformation(); // Check Information vector - // cout << AugInformationMatrix.size() << endl; Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { using namespace vanillaPose2; @@ -976,26 +963,25 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac smartFactor2->add(measurements_cam2, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, - 0.10); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); - graph.push_back(PriorFactor(x1, cam1, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, cam1); - values.insert(x2, Camera(pose2 * noise_pose, sharedK2)); + values.insert(x1, cam1.pose()); + values.insert(x2, pose2 * noise_pose); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, Camera(pose3 * noise_pose * noise_pose, sharedK2)); + values.insert(x3, pose3 * noise_pose * noise_pose); EXPECT( assert_equal( Pose3( @@ -1003,7 +989,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac -0.572308662, -0.324093872, 0.639358349, -0.521617766, -0.564921063), Point3(0.145118171, -0.252907438, 0.819956033)), - values.at(x3).pose())); + values.at(x3))); Values result; params.verbosityLM = LevenbergMarquardtParams::SUMMARY; @@ -1017,10 +1003,10 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac -0.572308662, -0.324093872, 0.639358349, -0.521617766, -0.564921063), Point3(0.145118171, -0.252907438, 0.819956033)), - result.at(x3).pose())); + result.at(x3))); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { using namespace vanillaPose; @@ -1067,20 +1053,20 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // 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, cam1); - values.insert(x2, cam2); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, Camera(pose3 * noise_pose, sharedK)); + values.insert(x3, pose3 * noise_pose); EXPECT( assert_equal( Pose3( @@ -1088,7 +1074,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) -0.130426831, -0.0115837907, 0.130819108, -0.98278564, -0.130455917), Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3).pose())); + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); @@ -1101,7 +1087,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) -0.130426831, -0.0115837907, 0.130819108, -0.98278564, -0.130455917), Point3(0.0897734171, -0.110201006, 0.901022872)), - result.at(x3).pose())); + result.at(x3))); } /* *************************************************************************/ @@ -1126,8 +1112,8 @@ TEST( SmartProjectionPoseFactor, Hessian ) { Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; - values.insert(x1, cam1); - values.insert(x2, cam2); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); boost::shared_ptr factor = smartFactor1->linearize(values); @@ -1156,9 +1142,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { smartFactorInstance->add(measurements_cam1, views, model); Values values; - values.insert(x1, cam1); - values.insert(x2, cam2); - values.insert(x3, cam3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); boost::shared_ptr factor = smartFactorInstance->linearize( values); @@ -1166,9 +1152,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; - rotValues.insert(x1, Camera(poseDrift.compose(level_pose), sharedK)); - rotValues.insert(x2, Camera(poseDrift.compose(pose_right), sharedK)); - rotValues.insert(x3, Camera(poseDrift.compose(pose_above), sharedK)); + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(pose_right)); + rotValues.insert(x3, poseDrift.compose(pose_above)); boost::shared_ptr factorRot = smartFactorInstance->linearize( rotValues); @@ -1180,16 +1166,15 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { Point3(10, -4, 5)); Values tranValues; - tranValues.insert(x1, Camera(poseDrift2.compose(level_pose), sharedK)); - tranValues.insert(x2, Camera(poseDrift2.compose(pose_right), sharedK)); - tranValues.insert(x3, Camera(poseDrift2.compose(pose_above), sharedK)); + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(pose_right)); + tranValues.insert(x3, poseDrift2.compose(pose_above)); boost::shared_ptr factorRotTran = smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT( - assert_equal(factor->information(), factorRotTran->information(), 1e-7)); + EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); } /* *************************************************************************/ @@ -1213,18 +1198,18 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { smartFactor->add(measurements_cam1, views, model); Values values; - values.insert(x1, cam1); - values.insert(x2, cam2); - values.insert(x3, cam3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); boost::shared_ptr factor = smartFactor->linearize(values); Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; - rotValues.insert(x1, Camera(poseDrift.compose(level_pose), sharedK2)); - rotValues.insert(x2, Camera(poseDrift.compose(level_pose), sharedK2)); - rotValues.insert(x3, Camera(poseDrift.compose(level_pose), sharedK2)); + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(level_pose)); + rotValues.insert(x3, poseDrift.compose(level_pose)); boost::shared_ptr factorRot = // smartFactor->linearize(rotValues); @@ -1236,16 +1221,15 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { Point3(10, -4, 5)); Values tranValues; - tranValues.insert(x1, Camera(poseDrift2.compose(level_pose), sharedK2)); - tranValues.insert(x2, Camera(poseDrift2.compose(level_pose), sharedK2)); - tranValues.insert(x3, Camera(poseDrift2.compose(level_pose), sharedK2)); + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(level_pose)); + tranValues.insert(x3, poseDrift2.compose(level_pose)); boost::shared_ptr factorRotTran = smartFactor->linearize( tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT( - assert_equal(factor->information(), factorRotTran->information(), 1e-7)); + EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); } /* ************************************************************************* */ @@ -1290,29 +1274,28 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1, noisePrior)); - graph.push_back(PriorFactor(x2, cam2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), 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, cam1); - values.insert(x2, cam2); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK)); + values.insert(x3, pose_above * noise_pose); EXPECT( assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3).pose())); + Point3(0.1, -0.1, 1.9)), values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - - EXPECT(assert_equal(cam3, result.at(x3), 1e-6)); + EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); } /* *************************************************************************/ @@ -1368,20 +1351,20 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // 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, cam1); - values.insert(x2, cam2); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, Camera(pose3 * noise_pose, sharedBundlerK)); + values.insert(x3, pose3 * noise_pose); EXPECT( assert_equal( Pose3( @@ -1389,7 +1372,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { -0.130426831, -0.0115837907, 0.130819108, -0.98278564, -0.130455917), Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3).pose())); + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); @@ -1402,9 +1385,8 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { -0.130426831, -0.0115837907, 0.130819108, -0.98278564, -0.130455917), Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3).pose())); + values.at(x3))); } -#endif /* ************************************************************************* */ int main() {