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

Fix more stuff in check.slam

release/4.3a0
Frank Dellaert 2022-02-16 09:43:12 -05:00
parent 65a3928d0a
commit 3d6a7bf970
2 changed files with 24 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
View File

@ -31,12 +31,13 @@ using namespace std;
using namespace boost::assign; using namespace boost::assign;
using namespace gtsam; using namespace gtsam;
namespace {
// make a realistic calibration matrix // make a realistic calibration matrix
static double b = 1; static double b = 1;
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
static Cal3_S2Stereo::shared_ptr K2( static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,
new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); b));
static SmartStereoProjectionParams params; static SmartStereoProjectionParams params;
@ -45,8 +46,8 @@ static SmartStereoProjectionParams params;
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
// Convenience for named keys // Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
using symbol_shorthand::X;
// tests data // tests data
static Symbol x1('X', 1); static Symbol x1('X', 1);
@ -59,16 +60,19 @@ static Symbol body_P_cam3_key('P', 3);
static Key poseKey1(x1); static Key poseKey1(x1);
static Key poseExtrinsicKey1(body_P_cam1_key); static Key poseExtrinsicKey1(body_P_cam1_key);
static Key poseExtrinsicKey2(body_P_cam2_key); static Key poseExtrinsicKey2(body_P_cam2_key);
static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? static StereoPoint2 measurement1(
static StereoPoint2 measurement2(350.0, 200.0, 240.0); //potentially use more reasonable measurement value? 323.0, 300.0, 240.0); // potentially use more reasonable measurement value?
static StereoPoint2 measurement2(
350.0, 200.0, 240.0); // potentially use more reasonable measurement value?
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0)); Point3(0.25, -0.10, 1.0));
static double missing_uR = std::numeric_limits<double>::quiet_NaN(); static double missing_uR = std::numeric_limits<double>::quiet_NaN();
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1, vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { const StereoCamera& cam2,
const StereoCamera& cam3,
Point3 landmark) {
vector<StereoPoint2> measurements_cam; vector<StereoPoint2> measurements_cam;
StereoPoint2 cam1_uv1 = cam1.project(landmark); StereoPoint2 cam1_uv1 = cam1.project(landmark);
@ -82,6 +86,7 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
} }
LevenbergMarquardtParams lm_params; LevenbergMarquardtParams lm_params;
} // namespace
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartStereoProjectionFactorPP, params) { TEST( SmartStereoProjectionFactorPP, params) {

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

@ -32,13 +32,13 @@ using namespace std;
using namespace boost::assign; using namespace boost::assign;
using namespace gtsam; using namespace gtsam;
namespace {
// make a realistic calibration matrix // make a realistic calibration matrix
static double b = 1; static double b = 1;
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
static Cal3_S2Stereo::shared_ptr K2( static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,
new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); b));
static SmartStereoProjectionParams params; static SmartStereoProjectionParams params;
@ -47,8 +47,8 @@ static SmartStereoProjectionParams params;
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
// Convenience for named keys // Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
using symbol_shorthand::X;
// tests data // tests data
static Symbol x1('X', 1); static Symbol x1('X', 1);
@ -56,15 +56,17 @@ static Symbol x2('X', 2);
static Symbol x3('X', 3); static Symbol x3('X', 3);
static Key poseKey1(x1); static Key poseKey1(x1);
static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? static StereoPoint2 measurement1(
323.0, 300.0, 240.0); // potentially use more reasonable measurement value?
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0)); Point3(0.25, -0.10, 1.0));
static double missing_uR = std::numeric_limits<double>::quiet_NaN(); static double missing_uR = std::numeric_limits<double>::quiet_NaN();
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1, vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { const StereoCamera& cam2,
const StereoCamera& cam3,
Point3 landmark) {
vector<StereoPoint2> measurements_cam; vector<StereoPoint2> measurements_cam;
StereoPoint2 cam1_uv1 = cam1.project(landmark); StereoPoint2 cam1_uv1 = cam1.project(landmark);
@ -78,6 +80,7 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
} }
LevenbergMarquardtParams lm_params; LevenbergMarquardtParams lm_params;
} // namespace
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, params) { TEST( SmartStereoProjectionPoseFactor, params) {