From b5236232772bdfcc0e327c155b0ff68bebbf3cf3 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 25 Aug 2021 22:22:53 -0400 Subject: [PATCH] still challenging to parse extrinsics --- gtsam/slam/SmartProjectionFactorP.h | 3 +- .../slam/tests/testSmartProjectionFactorP.cpp | 166 +++++++++--------- 2 files changed, 88 insertions(+), 81 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index a215890bc..495557c83 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -51,7 +51,6 @@ class SmartProjectionFactorP : public SmartProjectionFactor { private: typedef SmartProjectionFactor Base; typedef SmartProjectionFactorP This; - typedef CAMERA Camera; typedef typename CAMERA::CalibrationType CALIBRATION; protected: @@ -63,6 +62,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor { std::vector body_P_sensors_; public: + typedef CAMERA Camera; + typedef CameraSet Cameras; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 3ad4d62b6..4ba15264d 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -182,92 +182,98 @@ TEST( SmartProjectionFactorP, noisy ) { sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); - std::vector body_P_sensors; - body_P_sensors.push_back(Pose3::identity()); - body_P_sensors.push_back(Pose3::identity()); - KeyVector views {x1, x2}; - factor2->add(measurements, views, sharedKs, body_P_sensors); + factor2->add(measurements, views, sharedKs); double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } -///* *************************************************************************/ -//TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { -// using namespace vanillaPose; -// -// // create arbitrary body_T_sensor (transforms from sensor to body) -// Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); -// -// // These are the poses we want to estimate, from camera measurements -// const Pose3 sensor_T_body = body_T_sensor.inverse(); -// Pose3 wTb1 = cam1.pose() * sensor_T_body; -// Pose3 wTb2 = cam2.pose() * sensor_T_body; -// Pose3 wTb3 = cam3.pose() * sensor_T_body; -// -// // three landmarks ~5 meters infront of camera -// Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); -// -// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); -// -// // Create smart factors -// KeyVector views {x1, x2, x3}; -// -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setDegeneracyMode(IGNORE_DEGENERACY); -// params.setEnableEPI(false); -// -// SmartFactorP smartFactor1(model, sharedK, body_T_sensor, params); -// smartFactor1.add(measurements_cam1, views); -// -// SmartFactorP smartFactor2(model, sharedK, body_T_sensor, params); -// smartFactor2.add(measurements_cam2, views); -// -// SmartFactorP smartFactor3(model, sharedK, body_T_sensor, params); -// smartFactor3.add(measurements_cam3, views); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// // Put all factors in factor graph, adding priors -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, wTb1, noisePrior); -// graph.addPrior(x2, wTb2, noisePrior); -// -// // Check errors at ground truth poses -// Values gtValues; -// gtValues.insert(x1, wTb1); -// gtValues.insert(x2, wTb2); -// gtValues.insert(x3, wTb3); -// double actualError = graph.error(gtValues); -// double expectedError = 0.0; -// DOUBLES_EQUAL(expectedError, actualError, 1e-7) -// -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); -// Values values; -// values.insert(x1, wTb1); -// values.insert(x2, wTb2); -// // initialize third pose with some noise, we expect it to move back to -// // original pose3 -// values.insert(x3, wTb3 * noise_pose); -// -// LevenbergMarquardtParams lmParams; -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(wTb3, result.at(x3))); -//} -// +/* *************************************************************************/ +TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { + using namespace vanillaPose; + + // create arbitrary body_T_sensor (transforms from sensor to body) + Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); + + // These are the poses we want to estimate, from camera measurements + const Pose3 sensor_T_body = body_T_sensor.inverse(); + Pose3 wTb1 = cam1.pose() * sensor_T_body; + Pose3 wTb2 = cam2.pose() * sensor_T_body; + Pose3 wTb3 = cam3.pose() * sensor_T_body; + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + KeyVector views {x1, x2, x3}; + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setEnableEPI(false); + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + std::vector body_T_sensors; + body_T_sensors.push_back(body_T_sensor); + body_T_sensors.push_back(body_T_sensor); + body_T_sensors.push_back(body_T_sensor); + + SmartFactorP smartFactor1(model, params); + smartFactor1.add(measurements_cam1, views, sharedKs, body_T_sensors); + + SmartFactorP smartFactor2(model, params); + smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors); + + SmartFactorP smartFactor3(model, params); + smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors);; + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + // Put all factors in factor graph, adding priors + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, wTb1, noisePrior); + graph.addPrior(x2, wTb2, noisePrior); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, wTb1); + gtValues.insert(x2, wTb2); + gtValues.insert(x3, wTb3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); + Values values; + values.insert(x1, wTb1); + values.insert(x2, wTb2); + // initialize third pose with some noise, we expect it to move back to + // original pose3 + values.insert(x3, wTb3 * noise_pose); + + LevenbergMarquardtParams lmParams; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(wTb3, result.at(x3))); +} + ///* *************************************************************************/ //TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { //