test still failing
lcarlone
parent
e8db2b6b9b
commit
8b4a74efff
|
|
@ -192,12 +192,12 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
const Values& values, const double lambda = 0.0, bool diagonalDamping =
|
||||
false) const {
|
||||
|
||||
size_t nrKeys = keys_.size();
|
||||
size_t nrUniqueKeys = keys_.size();
|
||||
|
||||
// Create structures for Hessian Factors
|
||||
KeyVector js;
|
||||
std::vector<Matrix> Gs(nrKeys * (nrKeys + 1) / 2);
|
||||
std::vector<Vector> gs(nrKeys);
|
||||
std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
|
||||
std::vector<Vector> gs(nrUniqueKeys);
|
||||
|
||||
if (this->measured_.size() != cameras(values).size())
|
||||
throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
|
||||
|
|
@ -223,34 +223,28 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
|
||||
|
||||
// Whiten using noise model
|
||||
// std::cout << "noise model1 \n " << std::endl;
|
||||
noiseModel_->WhitenSystem(E, b);
|
||||
// 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;
|
||||
// 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;
|
||||
// marginalize point
|
||||
SymmetricBlockMatrix augmentedHessian = //
|
||||
Cameras::SchurComplement<3,Dim>(Fs, E, P, b);
|
||||
|
||||
std::vector<DenseIndex> dims(nrKeys + 1); // this also includes the b term
|
||||
// now pack into an Hessian factor
|
||||
std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term
|
||||
std::fill(dims.begin(), dims.end() - 1, 6);
|
||||
dims.back() = 1;
|
||||
|
||||
size_t nrNonuniqueKeys = w_P_body_keys_.size() + body_P_cam_keys_.size();
|
||||
SymmetricBlockMatrix augmentedHessianPP;
|
||||
if ( nrKeys == nrNonuniqueKeys ){ // 1 calibration per camera
|
||||
augmentedHessianPP = SymmetricBlockMatrix(dims, Matrix(augmentedHessian.selfadjointView()));
|
||||
}else{
|
||||
SymmetricBlockMatrix augmentedHessianUniqueKeys;
|
||||
if ( nrUniqueKeys == nrNonuniqueKeys ){ // if there is 1 calibration key per camera
|
||||
augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix(augmentedHessian.selfadjointView()));
|
||||
}else{ // if multiple cameras share a calibration
|
||||
std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term
|
||||
std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6);
|
||||
nonuniqueDims.back() = 1;
|
||||
|
|
@ -263,54 +257,55 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
nonuniqueKeys.push_back(body_P_cam_keys_.at(i));
|
||||
}
|
||||
|
||||
// get map from key to location in the new augmented Hessian matrix
|
||||
// get map from key to location in the new augmented Hessian matrix (the one including only unique keys)
|
||||
std::map<Key,size_t> keyToSlotMap;
|
||||
for(size_t k=0; k<nrKeys;k++){
|
||||
for(size_t k=0; k<nrUniqueKeys;k++){
|
||||
keyToSlotMap[keys_[k]] = k;
|
||||
}
|
||||
|
||||
std::cout << "linearize" << std::endl;
|
||||
for(size_t i=0; i<nrKeys;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;
|
||||
}
|
||||
// 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
|
||||
augmentedHessianPP = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrKeys+1,6*nrKeys+1));
|
||||
augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1));
|
||||
|
||||
std::cout <<" start for loop: " << std::endl;
|
||||
// add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian)
|
||||
for(size_t i=0; i<nrNonuniqueKeys;i++){ // rows
|
||||
Key key_i = nonuniqueKeys.at(i);
|
||||
std::cout <<" start for loop i: " << std::endl;
|
||||
|
||||
// update information vector
|
||||
augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i] , nrUniqueKeys,
|
||||
augmentedHessian.aboveDiagonalBlock(i,nrNonuniqueKeys));
|
||||
|
||||
// update blocks
|
||||
for(size_t j=0; j<nrNonuniqueKeys;j++){ // cols
|
||||
std::cout <<" start for loop j: " << std::endl;
|
||||
Key key_j = nonuniqueKeys.at(j);
|
||||
std::cout <<"key_i: " << DefaultKeyFormatter(key_i);
|
||||
std::cout <<" key_j: " << DefaultKeyFormatter(key_j);
|
||||
std::cout <<" start for loop --: " << std::endl;
|
||||
if(i==j){
|
||||
std::cout <<" i=0: " << std::endl;
|
||||
augmentedHessianPP.updateDiagonalBlock( keyToSlotMap[key_i] , augmentedHessian.diagonalBlock(i));
|
||||
augmentedHessianUniqueKeys.updateDiagonalBlock( keyToSlotMap[key_i] , augmentedHessian.diagonalBlock(i));
|
||||
}else if(i < j){
|
||||
std::cout <<" i<j: " << std::endl;
|
||||
augmentedHessianPP.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j],
|
||||
augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j],
|
||||
augmentedHessian.aboveDiagonalBlock(i,j));
|
||||
}
|
||||
else{
|
||||
std::cout <<" i>j: " << std::endl;
|
||||
augmentedHessianPP.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j],
|
||||
augmentedHessian.aboveDiagonalBlock(j,i));
|
||||
augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j],
|
||||
augmentedHessian.aboveDiagonalBlock(j,i).transpose());
|
||||
}
|
||||
}
|
||||
}
|
||||
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_, augmentedHessianPP);
|
||||
|
||||
return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, augmentedHessianUniqueKeys);
|
||||
//std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -609,6 +609,142 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_error_sameEx
|
|||
EXPECT_DOUBLES_EQUAL(0.0, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_error_sameExtrinsicKey_error_noisy ) {
|
||||
|
||||
// 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);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark1);
|
||||
vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark2);
|
||||
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark3);
|
||||
|
||||
double initialError_expected, initialError_actual;
|
||||
{
|
||||
KeyVector poseKeys;
|
||||
poseKeys.push_back(x1);
|
||||
poseKeys.push_back(x2);
|
||||
poseKeys.push_back(x3);
|
||||
|
||||
KeyVector extrinsicKeys;
|
||||
extrinsicKeys.push_back(body_P_cam1_key);
|
||||
extrinsicKeys.push_back(body_P_cam2_key);
|
||||
extrinsicKeys.push_back(body_P_cam3_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);
|
||||
|
||||
SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
|
||||
smartFactor2->add(measurements_l2, poseKeys, extrinsicKeys, K2);
|
||||
|
||||
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
|
||||
Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
|
||||
Pose3 body_Pose_cam2 = body_Pose_cam1;
|
||||
Pose3 body_Pose_cam3 = body_Pose_cam1;
|
||||
Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
|
||||
Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse());
|
||||
Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam3.inverse());
|
||||
|
||||
Values values;
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
values.insert(x1, w_Pose_body1);
|
||||
values.insert(x2, w_Pose_body2);
|
||||
values.insert(x3, w_Pose_body3);
|
||||
values.insert(body_P_cam1_key, body_Pose_cam1 * noise_pose);
|
||||
values.insert(body_P_cam2_key, body_Pose_cam2 * noise_pose);
|
||||
// initialize third calibration with some noise, we expect it to move back to original pose3
|
||||
values.insert(body_P_cam3_key, body_Pose_cam3 * noise_pose);
|
||||
|
||||
// Graph
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, w_Pose_body1, noisePrior);
|
||||
graph.addPrior(x2, w_Pose_body2, noisePrior);
|
||||
graph.addPrior(x3, w_Pose_body3, noisePrior);
|
||||
|
||||
initialError_expected = graph.error(values);
|
||||
}
|
||||
|
||||
{
|
||||
KeyVector poseKeys;
|
||||
poseKeys.push_back(x1);
|
||||
poseKeys.push_back(x2);
|
||||
poseKeys.push_back(x3);
|
||||
|
||||
KeyVector extrinsicKeys;
|
||||
extrinsicKeys.push_back(body_P_cam1_key);
|
||||
extrinsicKeys.push_back(body_P_cam1_key);
|
||||
extrinsicKeys.push_back(body_P_cam1_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);
|
||||
|
||||
SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
|
||||
smartFactor2->add(measurements_l2, poseKeys, extrinsicKeys, K2);
|
||||
|
||||
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
|
||||
Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
|
||||
Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
|
||||
Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam1.inverse());
|
||||
Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam1.inverse());
|
||||
|
||||
Values values;
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
values.insert(x1, w_Pose_body1);
|
||||
values.insert(x2, w_Pose_body2);
|
||||
values.insert(x3, w_Pose_body3);
|
||||
values.insert(body_P_cam1_key, body_Pose_cam1 * noise_pose);
|
||||
|
||||
// Graph
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, w_Pose_body1, noisePrior);
|
||||
graph.addPrior(x2, w_Pose_body2, noisePrior);
|
||||
graph.addPrior(x3, w_Pose_body3, noisePrior);
|
||||
|
||||
initialError_actual = graph.error(values);
|
||||
}
|
||||
|
||||
std::cout << " initialError_expected " << initialError_expected << std::endl;
|
||||
EXPECT_DOUBLES_EQUAL(initialError_expected, initialError_actual, 1e-7);
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization_sameExtrinsicKey ) {
|
||||
|
||||
|
|
@ -689,6 +825,10 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization
|
|||
double initialErrorSmart = graph.error(values);
|
||||
EXPECT_DOUBLES_EQUAL(31986.961831653316, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
|
||||
|
||||
std::cout << " 1: " << smartFactor1->error(values) <<std::endl;
|
||||
std::cout << " 2: " << smartFactor2->error(values) <<std::endl;
|
||||
std::cout << " 3: " << smartFactor3->error(values) <<std::endl;
|
||||
|
||||
Values result;
|
||||
gttic_(SmartStereoProjectionFactorPP);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
|
||||
|
|
|
|||
Loading…
Reference in New Issue