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

Smart projection factor working on Kitti

release/4.3a0
Luca Carlone 2013-08-14 19:12:23 +00:00
parent d7e6f43fa7
commit 0514ad39c6
3 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp
View File

@ -43,11 +43,14 @@
// Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp>
#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>
#include <fstream>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
namespace NM = gtsam::noiseModel;
using symbol_shorthand::X;
@ -161,8 +164,8 @@ int main(int argc, char** argv) {
// Load values from VO camera poses output
gtsam::Values::shared_ptr loaded_values = loadPoseValues(input_dir+"camera_poses.txt");
graph.add(Pose3Prior(X(0),loaded_values->at<Pose3>(X(0)), prior_model));
graph.add(Pose3Prior(X(1),loaded_values->at<Pose3>(X(1)), prior_model));
graph.push_back(Pose3Prior(X(0),loaded_values->at<Pose3>(X(0)), prior_model));
graph.push_back(Pose3Prior(X(1),loaded_values->at<Pose3>(X(1)), prior_model));
//graph.print("thegraph");
// Read in kitti dataset

8
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -209,9 +209,9 @@ namespace gtsam {
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
std::cout << "pose " << pose << std::endl;
// std::cout << "pose " << pose << std::endl;
PinholeCamera<CALIBRATION> camera(pose, *K_);
b.at(i) = ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
b.at(i) = - ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
noise_-> WhitenSystem(Hx.at(i), Hl.at(i), b.at(i));
f += b.at(i).squaredNorm();
}
@ -405,7 +405,7 @@ namespace gtsam {
boost::optional<Point3> point;
if (point_) {
point = point_;
//std::cout << "Using existing point " << *point << std::endl;
std::cout << "Using existing point " << *point << std::endl;
} else {
//std::cout << "Triangulate during error calc" << std::endl;
point = triangulatePoint3(cameraPoses, measured_, *K_);
@ -422,7 +422,7 @@ namespace gtsam {
Point2 reprojectionError(camera.project(*point) - measured_.at(i));
overallError += noise_->distance( reprojectionError.vector() );
}
return std::sqrt(overallError);
return overallError;
} else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error
std::cout << "WARNING: Could not triangulate during error calc" << std::endl;
return 0.0;

9
gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp
View File

@ -120,7 +120,7 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) {
/* ************************************************************************* */
TEST( SmartProjectionFactor, noisy ){
cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl;
cout << " ************************ SmartProjectionFactor: noisy ****************************" << endl;
Symbol x1('X', 1);
Symbol x2('X', 2);
@ -291,9 +291,11 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
resultWithSmartFactor.at<Pose3>(x3).print("\nSmart: Pose3 after optimization: ");
EXPECT(assert_equal(resultWithOriginalFactor.at<Pose3>(x3),resultWithSmartFactor.at<Pose3>(x3)));
std::cout << "\n================= STARTING GN ITERATION ========================" << std::endl;
GaussNewtonParams params2;
params2.maxIterations = 1;
Values resultWithOriginalFactor2;
params2.verbosity = NonlinearOptimizerParams::DELTA;
GaussNewtonOptimizer optimizerForOriginalFactor2(graphWithOriginalFactor, valuesOriginalFactor, params2);
resultWithOriginalFactor2 = optimizerForOriginalFactor2.optimize();
@ -301,6 +303,7 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
GaussNewtonOptimizer optimizerForSmartFactor2(graphWithSmartFactor, valuesSmartFactor, params2);
resultWithSmartFactor2 = optimizerForSmartFactor2.optimize();
std::cout << "\n=========================================" << std::endl;
resultWithOriginalFactor2.at<Pose3>(x3).print("Original: Pose3 after optimization (GaussNewton): ");
resultWithSmartFactor2.at<Pose3>(x3).print("\nSmart: Pose3 after optimization (GaussNewton): ");
@ -523,10 +526,6 @@ TEST( SmartProjectionFactor, Hessian ){
// compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
// check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
}
/* ************************************************************************* */