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

pipeline up and running, need to fix Jacobians next, then Schur complement

release/4.3a0
lcarlone 2021-03-13 22:34:37 -05:00
parent dbc10ff227
commit 2132778edc
2 changed files with 44 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -154,28 +154,18 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
if (!result_) { if (!result_) {
throw ("computeJacobiansWithTriangulatedPoint"); throw ("computeJacobiansWithTriangulatedPoint");
} else { } else {
// Matrix H0, H02;
// PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
// Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
// *H2 = *H1 * H02;
// *H1 = *H1 * H0;
// valid result: compute jacobians // valid result: compute jacobians
// const Pose3 sensor_P_body = body_P_sensor_->inverse(); Matrix H0,H1,H3,H02;
// constexpr int camera_dim = traits<CAMERA>::dimension; for (size_t i = 0; i < keys_.size(); i++) { // for each camera/measurement
// constexpr int pose_dim = traits<Pose3>::dimension; Pose3 w_P_body = values.at<Pose3>(keys_.at(i));
// Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_.at(i));
// for (size_t i = 0; i < Fs->size(); i++) { StereoCamera camera(w_P_body.compose(body_P_cam, H0, H02), K_all_[i]);
// const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; StereoPoint2 reprojectionError = StereoPoint2(camera.project(*result_, H1, H3) - measured_.at(i));
// Eigen::Matrix<double, camera_dim, camera_dim> J; Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12
// J.setZero(); J.block<ZDim,6>(0,0) = H1 * H0;
// Eigen::Matrix<double, pose_dim, pose_dim> H; J.block<ZDim,6>(0,6) = H1 * H02;
// // Call compose to compute Jacobian for camera extrinsics Fs.at(i) = J;
// world_P_body.compose(*body_P_sensor_, H); }
// // Assign extrinsics part of the Jacobian
// J.template block<pose_dim, pose_dim>(0, 0) = H;
// Fs->at(i) = Fs->at(i) * J;
// }
} }
} }
@ -197,6 +187,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
"measured_.size() inconsistent with input"); "measured_.size() inconsistent with input");
triangulateSafe(cameras(values)); triangulateSafe(cameras(values));
std::cout <<"passed triangulateSafe!!!!!!!!!!1" << std::endl;
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
// failed: return"empty" Hessian // failed: return"empty" Hessian
@ -212,6 +203,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
FBlocks Fs; FBlocks Fs;
Matrix F, E; Matrix F, E;
Vector b; Vector b;
std::cout <<"before computeJacobiansWithTriangulatedPoint!!!!!!!!!!1" << std::endl;
computeJacobiansWithTriangulatedPoint(Fs, E, b, values); computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
std::cout << Fs.at(0) << std::endl; std::cout << Fs.at(0) << std::endl;
@ -228,6 +220,28 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
Gs, gs, 0.0); Gs, gs, 0.0);
} }
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses and extrinsic pose for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
switch (params_.linearizationMode) {
case HESSIAN:
return createHessianFactor(values, lambda);
default:
throw std::runtime_error("SmartStereoFactorlinearize: unknown mode");
}
}
/// linearize
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return linearizeDamped(values);
}
private: private:
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;

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

@ -462,15 +462,15 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor ) {
Point3 landmark2_smart = *smartFactor2->point(); Point3 landmark2_smart = *smartFactor2->point();
Point3 landmark3_smart = *smartFactor3->point(); Point3 landmark3_smart = *smartFactor3->point();
// Values result; Values result;
// gttic_(SmartStereoProjectionFactorPP); gttic_(SmartStereoProjectionFactorPP);
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// result = optimizer.optimize(); result = optimizer.optimize();
// gttoc_(SmartStereoProjectionFactorPP); gttoc_(SmartStereoProjectionFactorPP);
// tictoc_finishedIteration_(); tictoc_finishedIteration_();
//
// // cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl; // cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl;
// EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); // GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
// VectorValues delta = GFG->optimize(); // VectorValues delta = GFG->optimize();