From a439cf0f0fe9858852d6a5c519db4da8eccb1d9a Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 23:33:42 -0400 Subject: [PATCH] stuck on compile issue --- .../SmartProjectionPoseFactorRollingShutter.h | 76 +++++++++++-------- ...martProjectionPoseFactorRollingShutter.cpp | 6 ++ 2 files changed, 50 insertions(+), 32 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 8bc923e20..a9f6d6bdf 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -18,6 +18,9 @@ #pragma once #include +#include +#include +#include namespace gtsam { /** @@ -58,6 +61,8 @@ PinholePose > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef PinholePose Camera; + // typedef CameraSet Cameras; + /// shorthand for base class type typedef SmartProjectionFactor Base; @@ -289,13 +294,12 @@ PinholePose > { } /// linearize and return a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + boost::shared_ptr > createHessianFactor( const Values& values, const double lambda = 0.0, bool diagonalDamping = false) const { - // we may have multiple cameras sharing the same extrinsic cals, hence the number - // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we - // have a body key and an extrinsic calibration key for each measurement) + // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation), + // hence the number of unique keys may be smaller than 2 * nrMeasurements size_t nrUniqueKeys = this->keys_.size(); size_t nrNonuniqueKeys = 2*world_P_body_key_pairs_.size(); @@ -304,37 +308,38 @@ PinholePose > { std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector gs(nrUniqueKeys); - if (this->measured_.size() != cameras(values).size()) + if (this->measured_.size() != this->cameras(values).size()) throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "measured_.size() inconsistent with input"); - // // triangulate 3D point at given linearization point - // triangulateSafe(cameras(values)); - // - // if (!this->result_) { // failed: return "empty/zero" Hessian - // for (Matrix& m : Gs) - // m = Matrix::Zero(DimPose, DimPose); - // for (Vector& v : gs) - // v = Vector::Zero(DimPose); - // return boost::make_shared < RegularHessianFactor - // > ( this->keys_, Gs, gs, 0.0); - // } - // - // // compute Jacobian given triangulated 3D Point - // FBlocks Fs; - // Matrix F, E; - // Vector b; - // computeJacobiansWithTriangulatedPoint(Fs, E, b, values); - // - // // Whiten using noise model - // this->noiseModel_->WhitenSystem(E, b); - // for (size_t i = 0; i < Fs.size(); i++) - // Fs[i] = this->noiseModel_->Whiten(Fs[i]); - // - // // build augmented Hessian (with last row/column being the information vector) - // Matrix3 P; - // This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); - // + std::cout << "got this far.." << std::endl; + + // triangulate 3D point at given linearization point + this->triangulateSafe(this->cameras(values)); + + if (!this->result_) { // failed: return "empty/zero" Hessian + for (Matrix& m : Gs) + m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) + v = Vector::Zero(DimPose); + return boost::make_shared < RegularHessianFactor > ( this->keys_, Gs, gs, 0.0); + } + + // compute Jacobian given triangulated 3D Point + FBlocks Fs; + Matrix F, E; + Vector b; + this->computeJacobiansWithTriangulatedPoint(Fs, E, b, values); + + // Whiten using noise model + this->noiseModel_->WhitenSystem(E, b); + for (size_t i = 0; i < Fs.size(); i++) + Fs[i] = this->noiseModel_->Whiten(Fs[i]); + + // build augmented Hessian (with last row/column being the information vector) + Matrix3 P; + Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); + // // marginalize point: note - we reuse the standard SchurComplement function // SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b); @@ -409,6 +414,13 @@ PinholePose > { // } // return boost::make_shared < RegularHessianFactor // > ( this->keys_, augmentedHessianUniqueKeys); + + // TO REMOVE: + for (Matrix& m : Gs) + m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) + v = Vector::Zero(DimPose); + return boost::make_shared < RegularHessianFactor > ( this->keys_, Gs, gs, 0.0); } /** diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index d04921424..602ca155e 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -352,6 +352,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) 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); + EXPECT( // check that the pose is actually noisy + 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))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);