12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

now I have a working prototype!

release/4.3a0
lcarlone 2021-03-27 22:28:35 -04:00
parent 8b4a74efff
commit 8ca3d475c8
2 changed files with 74 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -273,6 +273,24 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
// std::cout <<" key slot: " << keyToSlotMap[nonuniqueKeys[i]] << std::endl; // std::cout <<" key slot: " << keyToSlotMap[nonuniqueKeys[i]] << std::endl;
// } // }
/////////////////////////////////////////////////////////////////
// PROTOTYPING
size_t numMeasurements = measured_.size();
Matrix F = Matrix::Zero(3*numMeasurements, 6 * nrUniqueKeys);
for(size_t k=0; k<numMeasurements; k++){
Key key_body = w_P_body_keys_.at(k);
Key key_cal = body_P_cam_keys_.at(k);
F.block<3,6>( 3*k , 6*keyToSlotMap[key_body] ) = Fs[k].block<3,6>(0,0);
F.block<3,6>( 3*k , 6*keyToSlotMap[key_cal] ) = Fs[k].block<3,6>(0,6);
}
Matrix augH = Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1);
augH.block(0,0,6*nrUniqueKeys,6*nrUniqueKeys) = F.transpose() * F - F.transpose() * E * P * E.transpose() * F;
Matrix infoVec = F.transpose() * b - F.transpose() * E * P * E.transpose() * b;
augH.block(0,6*nrUniqueKeys,6*nrUniqueKeys,1) = infoVec;
augH.block(6*nrUniqueKeys,0,1,6*nrUniqueKeys) = infoVec.transpose();
augH(6*nrUniqueKeys,6*nrUniqueKeys) = b.squaredNorm();
/////////////////////////////////////////////////////////////////
// initialize matrix to zero // initialize matrix to zero
augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1)); augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1));
@ -301,8 +319,13 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
} }
augmentedHessianUniqueKeys.updateDiagonalBlock(nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); augmentedHessianUniqueKeys.updateDiagonalBlock(nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
std::cout << "MAtrix \n " << Matrix(augmentedHessianUniqueKeys.selfadjointView()) <<std::endl; //std::cout << "Matrix \n " << Matrix(augmentedHessianUniqueKeys.selfadjointView()) <<std::endl;
std::cout << "sq norm " << b.squaredNorm() << std::endl; //std::cout << "sq norm " << b.squaredNorm() << std::endl;
// std::cout << "norm diff: "<< Matrix(augH - Matrix(augmentedHessianUniqueKeys.selfadjointView())) << std::endl;
//
// std::cout << "TEST MATRIX:" << std::endl;
augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, augH);
} }
return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, augmentedHessianUniqueKeys); return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, augmentedHessianUniqueKeys);

50
gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
View File

@ -745,6 +745,54 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_error_sameEx
EXPECT_DOUBLES_EQUAL(initialError_expected, initialError_actual, 1e-7); EXPECT_DOUBLES_EQUAL(initialError_expected, initialError_actual, 1e-7);
} }
/* *************************************************************************/
//TEST( SmartStereoProjectionFactorPP, smart_projection_factor_linearize_sameExtrinsicKey ) {
//
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
// Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
// StereoCamera cam1(w_Pose_cam1, K2);
//
// // create second camera 1 meter to the right of first camera
// Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
// StereoCamera cam2(w_Pose_cam2, K2);
//
// // create third camera 1 meter above the first camera
// Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
// StereoCamera cam3(w_Pose_cam3, K2);
//
// // three landmarks ~5 meters infront of camera
// Point3 landmark1(5, 0.5, 1.2);
//
// // 1. Project three landmarks into three cameras and triangulate
// vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
// cam2, cam3, landmark1);
//
// KeyVector poseKeys;
// poseKeys.push_back(x1);
// poseKeys.push_back(x2);
// poseKeys.push_back(x3);
//
// Symbol body_P_cam_key('P', 0);
// KeyVector extrinsicKeys;
// extrinsicKeys.push_back(body_P_cam_key);
// extrinsicKeys.push_back(body_P_cam_key);
// extrinsicKeys.push_back(body_P_cam_key);
//
// SmartStereoProjectionParams smart_params;
// smart_params.triangulation.enableEPI = true;
// SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
// smartFactor1->add(measurements_l1, poseKeys, extrinsicKeys, K2);
//
// // 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());
//
// // Hessian
//
//}
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization_sameExtrinsicKey ) { TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization_sameExtrinsicKey ) {
@ -823,7 +871,7 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization
// cost is large before optimization // cost is large before optimization
double initialErrorSmart = graph.error(values); double initialErrorSmart = graph.error(values);
EXPECT_DOUBLES_EQUAL(31986.961831653316, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error EXPECT_DOUBLES_EQUAL(31987.075556114589, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
std::cout << " 1: " << smartFactor1->error(values) <<std::endl; std::cout << " 1: " << smartFactor1->error(values) <<std::endl;
std::cout << " 2: " << smartFactor2->error(values) <<std::endl; std::cout << " 2: " << smartFactor2->error(values) <<std::endl;