got to the final monster. Now I need to implement createHessian
lcarlone
parent
e6ff03f73e
commit
d4b88ba59a
|
|
@ -98,7 +98,7 @@ PinholePose<CALIBRATION> > {
|
|||
*/
|
||||
void add(const Point2& measured, const Key& world_P_body_key1,
|
||||
const Key& world_P_body_key2, const double& gamma,
|
||||
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor) {
|
||||
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
|
||||
// store measurements in base class (note: we manyally add keys below to make sure they are unique
|
||||
this->measured_.push_back(measured);
|
||||
|
||||
|
|
@ -131,7 +131,7 @@ PinholePose<CALIBRATION> > {
|
|||
* @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated
|
||||
* @param Ks vector of intrinsic calibration objects
|
||||
*/
|
||||
void add(const std::vector<Point2>& measurements,
|
||||
void add(const Point2Vector& measurements,
|
||||
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
|
||||
const std::vector<double>& gammas,
|
||||
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
|
||||
|
|
@ -154,10 +154,10 @@ PinholePose<CALIBRATION> > {
|
|||
* @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated
|
||||
* @param K the (known) camera calibration (same for all measurements)
|
||||
*/
|
||||
void add(const std::vector<Point2>& measurements,
|
||||
void add(const Point2Vector& measurements,
|
||||
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
|
||||
const std::vector<double>& gammas,
|
||||
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor) {
|
||||
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
|
||||
assert(world_P_body_key_pairs.size() == measurements.size());
|
||||
assert(world_P_body_key_pairs.size() == gammas.size());
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
|
|
|
|||
|
|
@ -98,7 +98,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
|
|||
using namespace vanillaPose;
|
||||
|
||||
// create fake measurements
|
||||
std::vector<Point2> measurements;
|
||||
Point2Vector measurements;
|
||||
measurements.push_back(measurement1);
|
||||
measurements.push_back(measurement2);
|
||||
measurements.push_back(measurement3);
|
||||
|
|
@ -170,7 +170,6 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
|
|||
}
|
||||
|
||||
static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated
|
||||
static const int DimPose = 6; ///< Pose3 dimension
|
||||
static const int ZDim = 2; ///< Measurement dimension (Point2)
|
||||
typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt camera)
|
||||
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
|
||||
|
|
@ -298,68 +297,69 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
|
|||
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) {
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) {
|
||||
std::cout << "===================" << std::endl;
|
||||
|
||||
using namespace vanillaPose2;
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
using namespace vanillaPoseRS;
|
||||
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);
|
||||
// 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);
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
// create inputs
|
||||
std::vector<std::pair<Key,Key>> key_pairs;
|
||||
key_pairs.push_back(std::make_pair(x1,x2));
|
||||
key_pairs.push_back(std::make_pair(x2,x3));
|
||||
key_pairs.push_back(std::make_pair(x3,x1));
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
std::vector<double> interp_factors;
|
||||
interp_factors.push_back(interp_factor1);
|
||||
interp_factors.push_back(interp_factor2);
|
||||
interp_factors.push_back(interp_factor3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
SmartFactorRS smartFactor1(model);
|
||||
smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
SmartFactorRS smartFactor2(model);
|
||||
smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
SmartFactorRS smartFactor3(model);
|
||||
smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, cam1.pose(), noisePrior);
|
||||
graph.addPrior(x2, cam2.pose(), noisePrior);
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
Values groundTruth;
|
||||
groundTruth.insert(x1, cam1.pose());
|
||||
groundTruth.insert(x2, cam2.pose());
|
||||
groundTruth.insert(x3, cam3.pose());
|
||||
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, level_pose, noisePrior);
|
||||
graph.addPrior(x2, pose_right, 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.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, 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<Pose3>(x3)));
|
||||
Values groundTruth;
|
||||
groundTruth.insert(x1, level_pose);
|
||||
groundTruth.insert(x2, pose_right);
|
||||
groundTruth.insert(x3, pose_above);
|
||||
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
|
||||
}
|
||||
// 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, level_pose);
|
||||
values.insert(x2, pose_right);
|
||||
// initialize third pose with some noise, we expect it to move back to original pose_above
|
||||
values.insert(x3, pose_above * noise_pose);
|
||||
|
||||
/* *************************************************************************
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, Factors ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
|
|
|||
Loading…
Reference in New Issue