fixed print, removed cout, test still failing
lcarlone
parent
4df78be0f0
commit
7c052ff48a
|
|
@ -76,7 +76,7 @@ void SmartStereoProjectionFactorPP::add(
|
|||
|
||||
void SmartStereoProjectionFactorPP::print(
|
||||
const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s << "SmartStereoProjectionFactorPP, z = \n ";
|
||||
std::cout << s << "SmartStereoProjectionFactorPP: \n ";
|
||||
for (size_t i = 0; i < K_all_.size(); i++) {
|
||||
K_all_[i]->print("calibration = ");
|
||||
std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl;
|
||||
|
|
|
|||
|
|
@ -169,16 +169,16 @@ 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;
|
||||
// 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;
|
||||
// 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;
|
||||
// std::cout << "J \n" << J << std::endl;
|
||||
Fs.push_back(J);
|
||||
size_t row = 3*i;
|
||||
b.segment<ZDim>(row) = - reprojectionError.vector();
|
||||
|
|
@ -205,7 +205,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
|
||||
std::vector<Vector> gs(numKeys);
|
||||
|
||||
std::cout <<"using my hessian!!!!!!!!!!1" << std::endl;
|
||||
for(size_t i=0; i<numKeys;i++){
|
||||
std::cout <<"key: " << DefaultKeyFormatter(allKeys[i]) << std::endl;
|
||||
}
|
||||
|
|
@ -215,9 +214,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
"measured_.size() inconsistent with input");
|
||||
|
||||
triangulateSafe(cameras(values));
|
||||
std::cout <<"passed triangulateSafe!!!!!!!!!!1" << std::endl;
|
||||
|
||||
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
|
||||
std::cout << "degenerate" << std::endl;
|
||||
// failed: return"empty" Hessian
|
||||
for(Matrix& m: Gs)
|
||||
m = Matrix::Zero(DimPose,DimPose);
|
||||
|
|
@ -226,48 +225,46 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys,
|
||||
Gs, gs, 0.0);
|
||||
}
|
||||
|
||||
// std::cout << "result_" << *result_ << std::endl;
|
||||
// std::cout << "result_2" << result_ << std::endl;
|
||||
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
|
||||
FBlocks Fs;
|
||||
Matrix F, E;
|
||||
Vector b;
|
||||
std::cout <<"before computeJacobiansWithTriangulatedPoint!!!!!!!!!!1" << std::endl;
|
||||
computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
|
||||
std::cout << "Fs.at(0) \n"<< Fs.at(0) << std::endl;
|
||||
|
||||
std::cout << "Dim "<< Dim << std::endl;
|
||||
std::cout << "numKeys "<< numKeys << std::endl;
|
||||
|
||||
std::cout << "Fs.size() = " << Fs.size() << std::endl;
|
||||
std::cout << "E = " << E << std::endl;
|
||||
std::cout << "b = " << b << std::endl;
|
||||
// std::cout << "Dim "<< Dim << std::endl;
|
||||
// std::cout << "numKeys "<< numKeys << std::endl;
|
||||
//
|
||||
// std::cout << "Fs.size() = " << Fs.size() << std::endl;
|
||||
// std::cout << "E = " << E << std::endl;
|
||||
// std::cout << "b = " << b << std::endl;
|
||||
|
||||
// Whiten using noise model
|
||||
std::cout << "noise model1 \n " << std::endl;
|
||||
// std::cout << "noise model1 \n " << std::endl;
|
||||
noiseModel_->WhitenSystem(E, b);
|
||||
std::cout << "noise model2 \n " << std::endl;
|
||||
// std::cout << "noise model2 \n " << std::endl;
|
||||
for (size_t i = 0; i < Fs.size(); i++)
|
||||
Fs[i] = noiseModel_->Whiten(Fs[i]);
|
||||
|
||||
std::cout << "noise model3 \n " << std::endl;
|
||||
// std::cout << "noise model3 \n " << std::endl;
|
||||
// build augmented hessian
|
||||
Matrix3 P;
|
||||
Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
|
||||
|
||||
std::cout << "ComputePointCovariance done!!! \n " << std::endl;
|
||||
std::cout << "Fs.size() = " << Fs.size() << std::endl;
|
||||
std::cout << "E = " << E << std::endl;
|
||||
std::cout << "P = " << P << std::endl;
|
||||
std::cout << "b = " << b << std::endl;
|
||||
// std::cout << "ComputePointCovariance done!!! \n " << std::endl;
|
||||
// std::cout << "Fs.size() = " << Fs.size() << std::endl;
|
||||
// std::cout << "E = " << E << std::endl;
|
||||
// std::cout << "P = " << P << std::endl;
|
||||
// std::cout << "b = " << b << std::endl;
|
||||
SymmetricBlockMatrix augmentedHessian = //
|
||||
Cameras::SchurComplement<3,Dim>(Fs, E, P, b);
|
||||
|
||||
std::cout << "Repackaging!!! \n " << std::endl;
|
||||
|
||||
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
|
||||
std::fill(dims.begin(), dims.end() - 1, 6);
|
||||
dims.back() = 1;
|
||||
SymmetricBlockMatrix augmentedHessianPP(dims, Matrix(augmentedHessian.selfadjointView()));
|
||||
std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl;
|
||||
|
||||
return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys,
|
||||
augmentedHessianPP);
|
||||
|
|
|
|||
|
|
@ -659,22 +659,14 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization
|
|||
SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
|
||||
smartFactor3->add(measurements_l3, poseKeys, extrinsicKeys, K2);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
// Values
|
||||
// relevant poses:
|
||||
Pose3 body_Pose_cam = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
|
||||
Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam.inverse());
|
||||
Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam.inverse());
|
||||
Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam.inverse());
|
||||
|
||||
Values values;
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
|
||||
values.insert(x1, w_Pose_body1);
|
||||
values.insert(x2, w_Pose_body2);
|
||||
values.insert(x3, w_Pose_body3);
|
||||
values.insert(body_P_cam_key, body_Pose_cam*noise_pose);
|
||||
|
||||
// Graph
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
|
|
@ -683,7 +675,15 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization
|
|||
graph.addPrior(x2, w_Pose_body2, noisePrior);
|
||||
graph.addPrior(x3, w_Pose_body3, noisePrior);
|
||||
// we might need some prior on the calibration too
|
||||
// graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this!
|
||||
graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this!
|
||||
|
||||
// Values
|
||||
Values values;
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
|
||||
values.insert(x1, w_Pose_body1);
|
||||
values.insert(x2, w_Pose_body2);
|
||||
values.insert(x3, w_Pose_body3);
|
||||
values.insert(body_P_cam_key, body_Pose_cam*noise_pose);
|
||||
|
||||
// cost is large before optimization
|
||||
double initialErrorSmart = graph.error(values);
|
||||
|
|
@ -697,6 +697,12 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization
|
|||
tictoc_finishedIteration_();
|
||||
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
|
||||
|
||||
GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
|
||||
GFG->print("GFG \n");
|
||||
VectorValues delta = GFG->optimize();
|
||||
VectorValues expected = VectorValues::Zero(delta);
|
||||
EXPECT(assert_equal(expected, delta, 1e-6));
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
|
|
|
|||
Loading…
Reference in New Issue