need to clean up templates and remove 2 redundant lines
parent
2e1ed2c852
commit
5677bdb6c1
|
@ -449,10 +449,10 @@ public:
|
|||
/**
|
||||
* This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
|
||||
*/
|
||||
template<int D, int ZD> // D: camera dim, ZD: measurement dim
|
||||
void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
|
||||
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
|
||||
boost::optional<Matrix&> E = boost::none) const {
|
||||
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
|
||||
boost::optional<Matrix&> E = boost::none) const override
|
||||
{
|
||||
// when using stereo cameras, some of the measurements might be missing:
|
||||
for(size_t i=0; i < cameras.size(); i++){
|
||||
const StereoPoint2& z = measured_.at(i);
|
||||
|
@ -460,28 +460,18 @@ public:
|
|||
{
|
||||
if(Fs){ // delete influence of right point on jacobian Fs
|
||||
MatrixZD& Fi = Fs->at(i);
|
||||
for(size_t ii=0; ii<D; ii++)
|
||||
for(size_t ii=0; ii<Dim; ii++)
|
||||
Fi(1,ii) = 0.0;
|
||||
}
|
||||
if(E) // delete influence of right point on jacobian E
|
||||
E->row(ZD * i + 1) = Matrix::Zero(1, E->cols());
|
||||
E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
|
||||
|
||||
// set the corresponding entry of vector ue to zero
|
||||
ue(ZD * i + 1) = 0.0;
|
||||
ue(ZDim * i + 1) = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
|
||||
* This is class implementation that directly uses the measurement and camera size without templates.
|
||||
*/
|
||||
void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
|
||||
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
|
||||
boost::optional<Matrix&> E = boost::none) const override {
|
||||
correctForMissingMeasurements<Dim,ZDim>(cameras, ue, Fs, E);
|
||||
}
|
||||
|
||||
/** return the landmark */
|
||||
TriangulationResult point() const {
|
||||
return result_;
|
||||
|
|
|
@ -152,7 +152,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
/// Compute F, E only (called below in both vanilla and SVD versions)
|
||||
/// Assumes the point has been computed
|
||||
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
|
||||
void computeJacobiansWithTriangulatedPoint(
|
||||
void computeJacobiansAndCorrectForMissingMeasurements(
|
||||
FBlocks& Fs,
|
||||
Matrix& E, Vector& b, const Values& values) const {
|
||||
if (!result_) {
|
||||
|
@ -168,23 +168,20 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_.at(i));
|
||||
StereoCamera camera(w_P_body.compose(body_P_cam, dPoseCam_dPoseBody, dPoseCam_dPoseExt), K_all_[i]);
|
||||
StereoPoint2 reprojectionError = StereoPoint2(camera.project(*result_, dProject_dPoseCam, Ei) - measured_.at(i));
|
||||
// std::cout << "H0 \n" << dPoseCam_dPoseBody << std::endl;
|
||||
// std::cout << "H1 \n" << dProject_dPoseCam << std::endl;
|
||||
// std::cout << "H3 \n" << Ei << std::endl;
|
||||
// std::cout << "H02 \n" << dPoseCam_dPoseExt << std::endl;
|
||||
Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12
|
||||
// std::cout << "H1 * H0 \n" << dProject_dPoseCam * dPoseCam_dPoseBody << std::endl;
|
||||
// std::cout << "H1 * H02 \n" << dProject_dPoseCam * dPoseCam_dPoseExt << std::endl;
|
||||
J.block<ZDim,6>(0,0) = dProject_dPoseCam * dPoseCam_dPoseBody; // (3x6) * (6x6)
|
||||
J.block<ZDim,6>(0,6) = dProject_dPoseCam * dPoseCam_dPoseExt; // (3x6) * (6x6)
|
||||
// std::cout << "J \n" << J << std::endl;
|
||||
if(std::isnan(measured_.at(i).uR())) // if the right pixel is invalid
|
||||
{
|
||||
J.block<1,12>(1,0) = Matrix::Zero(1,12);
|
||||
Ei.block<1,3>(1,0) = Matrix::Zero(1,3);
|
||||
reprojectionError = StereoPoint2(reprojectionError.uL(), 0.0, reprojectionError.v() );
|
||||
}
|
||||
Fs.push_back(J);
|
||||
size_t row = 3*i;
|
||||
b.segment<ZDim>(row) = - reprojectionError.vector();
|
||||
E.block<3,3>(row,0) = Ei;
|
||||
}
|
||||
// correct for monocular measurements, where the right pixel measurement is nan
|
||||
//Base::CorrectForMissingMeasurements(measured_, cameras, b, Fs, E);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -204,11 +201,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
|
||||
"measured_.size() inconsistent with input");
|
||||
|
||||
std::cout << "triangulate" << std::endl;
|
||||
triangulateSafe(cameras(values));
|
||||
|
||||
if (!result_) {
|
||||
std::cout << "degenerate" << std::endl;
|
||||
// failed: return"empty" Hessian
|
||||
for(Matrix& m: Gs)
|
||||
m = Matrix::Zero(DimPose,DimPose);
|
||||
|
@ -218,12 +213,11 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
Gs, gs, 0.0);
|
||||
}
|
||||
|
||||
std::cout << "params_.degeneracyMode" << params_.degeneracyMode << std::endl;
|
||||
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
|
||||
FBlocks Fs;
|
||||
Matrix F, E;
|
||||
Vector b;
|
||||
computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
|
||||
computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values);
|
||||
|
||||
// Whiten using noise model
|
||||
noiseModel_->WhitenSystem(E, b);
|
||||
|
@ -266,16 +260,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
keyToSlotMap[keys_[k]] = k;
|
||||
}
|
||||
|
||||
std::cout << "linearize" << std::endl;
|
||||
// for(size_t i=0; i<nrUniqueKeys;i++){
|
||||
// std::cout <<"key: " << DefaultKeyFormatter(keys_[i]);
|
||||
// std::cout <<" key slot: " << keyToSlotMap[keys_[i]] << std::endl;
|
||||
// }
|
||||
// for(size_t i=0; i<nrNonuniqueKeys;i++){
|
||||
// std::cout <<"key: " << DefaultKeyFormatter(nonuniqueKeys[i]);
|
||||
// std::cout <<" key slot: " << keyToSlotMap[nonuniqueKeys[i]] << std::endl;
|
||||
// }
|
||||
|
||||
// initialize matrix to zero
|
||||
augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1));
|
||||
|
||||
|
@ -309,13 +293,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
}
|
||||
}
|
||||
augmentedHessianUniqueKeys.updateDiagonalBlock(nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
|
||||
|
||||
//std::cout << "Matrix \n " << Matrix(augmentedHessianUniqueKeys.selfadjointView()) <<std::endl;
|
||||
//std::cout << "sq norm " << b.squaredNorm() << std::endl;
|
||||
}
|
||||
|
||||
return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, augmentedHessianUniqueKeys);
|
||||
//std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -862,7 +862,7 @@ TEST( SmartStereoProjectionFactorPP, 3poses_optimization_sameExtrinsicKey ) {
|
|||
EXPECT(assert_equal(expected, delta, 1e-4));
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionFactorPP, monocular_multipleExtrinsicKeys ){
|
||||
// make a realistic calibration matrix
|
||||
double fov = 60; // degrees
|
||||
|
@ -979,7 +979,7 @@ TEST( SmartStereoProjectionFactorPP, monocular_multipleExtrinsicKeys ){
|
|||
tictoc_finishedIteration_();
|
||||
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
|
||||
EXPECT(assert_equal(sensor_to_body,result.at<Pose3>(body_P_cam_key)));
|
||||
EXPECT(assert_equal(sensor_to_body,result.at<Pose3>(body_P_cam_key), 1e-1));
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
|
|
Loading…
Reference in New Issue