still challenging to parse extrinsics
lcarlone
parent
7b399a363a
commit
b523623277
|
|
@ -51,7 +51,6 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
|
|||
private:
|
||||
typedef SmartProjectionFactor<CAMERA> Base;
|
||||
typedef SmartProjectionFactorP<CAMERA> This;
|
||||
typedef CAMERA Camera;
|
||||
typedef typename CAMERA::CalibrationType CALIBRATION;
|
||||
|
||||
protected:
|
||||
|
|
@ -63,6 +62,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
|
|||
std::vector<Pose3> body_P_sensors_;
|
||||
|
||||
public:
|
||||
typedef CAMERA Camera;
|
||||
typedef CameraSet<CAMERA> Cameras;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
|
|
|||
|
|
@ -182,92 +182,98 @@ TEST( SmartProjectionFactorP, noisy ) {
|
|||
sharedKs.push_back(sharedK);
|
||||
sharedKs.push_back(sharedK);
|
||||
|
||||
std::vector<Pose3> 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<Pose3>(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<boost::shared_ptr<Cal3_S2>> sharedKs;
|
||||
sharedKs.push_back(sharedK);
|
||||
sharedKs.push_back(sharedK);
|
||||
sharedKs.push_back(sharedK);
|
||||
|
||||
std::vector<Pose3> 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<Pose3>(x3)));
|
||||
}
|
||||
|
||||
///* *************************************************************************/
|
||||
//TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) {
|
||||
//
|
||||
|
|
|
|||
Loading…
Reference in New Issue