diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 5772e7822..e42a61d5c 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -171,8 +171,13 @@ namespace gtsam { /// linearize returns a Hessianfactor that is an approximation of error(p) virtual boost::shared_ptr linearize(const Values& values) const { - bool debug = false; - bool blockwise = true; + bool blockwise = false; + + unsigned int numKeys = keys_.size(); + std::vector js; + std::vector Gs(numKeys*(numKeys+1)/2); + std::vector gs(numKeys); + double f=0; // Collect all poses (Cameras) std::vector cameraPoses; @@ -184,53 +189,25 @@ namespace gtsam { } // We triangulate the 3D position of the landmark - if (debug) { - BOOST_FOREACH(const Pose3& pose, cameraPoses) { - std::cout << "Pose: " << pose << std::endl; - } - BOOST_FOREACH(const Point2& point, measured_) { - std::cout << "Point: " << point << std::endl; - } - } boost::optional point; - if (point_) { - point = point_; - //std::cout << "Using existing point " << *point << std::endl; - } else { - //std::cout << "Triangulating in linearize " << std::endl; - point = triangulatePoint3(cameraPoses, measured_, *K_); - } - if (debug) std::cout << "Result: " << *point << std::endl; - - - if (debug) { - std::cout << "point " << *point << std::endl; + try { + point = triangulatePoint3(cameraPoses, measured_, *K_); + } catch( TriangulationCheiralityException& e) { + // point is behind one of the cameras, turn factor off by setting everything to 0 + //std::cout << e.what() << std::end; + BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6); + BOOST_FOREACH(Vector& v, gs) v = zero(6); + return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); } - std::vector Gs(keys_.size()*(keys_.size()+1)/2); - std::vector gs(keys_.size()); - double f=0; - - // point is behind one of the cameras, turn factor off by setting everything to 0 - if (!point) { - std::cout << "WARNING: Could not triangulate during linearize" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6,6); - BOOST_FOREACH(Vector& v, gs) v = zero(6); - return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); - } - - // For debug only - std::vector Gs1; - std::vector gs1; - if (blockwise || debug){ + if (blockwise){ // ========================================================================================================== - std::vector Hx(keys_.size()); - std::vector Hl(keys_.size()); - std::vector b(keys_.size()); + std::vector Hx(numKeys); + std::vector Hl(numKeys); + std::vector b(numKeys); for(size_t i = 0; i < measured_.size(); i++) { Pose3 pose = cameraPoses.at(i); -// std::cout << "pose " << pose << std::endl; PinholeCamera camera(pose, *K_); 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)); @@ -255,64 +232,33 @@ namespace gtsam { for(size_t i2 = 0; i2 < keys_.size(); i2++) { // we only need the upper triangular entries Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose(); - if (i1==0 && i2==0){ - if (debug) { - std::cout << "Hoff"<< i1 << i2 << "=[" << Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose() << "];" << std::endl; - std::cout << "Hxoff"<< "=[" << Hx.at(i1) << "];" << std::endl; - std::cout << "Hloff"<< "=[" << Hl.at(i1) << "];" << std::endl; - std::cout << "Hloff2"<< "=[" << Hl.at(i2) << "];" << std::endl; - std::cout << "C"<< "=[" << C << "];" << std::endl; - } - } } } // Populate Gs and gs int GsCount = 0; - for(size_t i1 = 0; i1 < keys_.size(); i1++) { + for(size_t i1 = 0; i1 < numKeys; i1++) { gs.at(i1) = Hx.at(i1).transpose() * b.at(i1); - for(size_t i2 = 0; i2 < keys_.size(); i2++) { + for(size_t i2 = 0; i2 < numKeys; i2++) { gs.at(i1) -= Hxl[i1][i2] * b.at(i2); if (i2 == i1){ Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2); - - if (debug) { - std::cout << "HxlH"<< GsCount << "=[" << Hxl[i1][i2] * Hx.at(i2) << "];" << std::endl; - std::cout << "Hx2_"<< GsCount << "=[" << Hx.at(i2) << "];" << std::endl; - std::cout << "H"<< GsCount << "=[" << Gs.at(GsCount) << "];" << std::endl; - } GsCount++; } if (i2 > i1) { Gs.at(GsCount) = - Hxl[i1][i2] * Hx.at(i2); - - if (debug) { - std::cout << "HxlH"<< GsCount << "=[" << Hxl[i1][i2] * Hx.at(i2) << "];" << std::endl; - std::cout << "Hx2_"<< GsCount << "=[" << Hx.at(i2) << "];" << std::endl; - std::cout << "H"<< GsCount << "=[" << Gs.at(GsCount) << "];" << std::endl; - } GsCount++; } } } - if (debug) { - // Copy result for later comparison - BOOST_FOREACH(const Matrix& m, Gs) { - Gs1.push_back(m); - } - // Copy result for later comparison - BOOST_FOREACH(const Matrix& m, gs) { - gs1.push_back(m); - } - } } - if (blockwise == false || debug){ // version with full matrix multiplication + if (blockwise == false){ // version with full matrix multiplication // ========================================================================================================== - Matrix Hx2 = zeros(2*keys_.size(), 6*keys_.size()); - Matrix Hl2 = zeros(2*keys_.size(), 3); - Vector b2 = zero(2*keys_.size()); + Matrix Hx2 = zeros(2 * numKeys, 6 * numKeys); + Matrix Hl2 = zeros(2 * numKeys, 3); + Vector b2 = zero(2 * numKeys); for(size_t i = 0; i < measured_.size(); i++) { Pose3 pose = cameraPoses.at(i); @@ -326,39 +272,24 @@ namespace gtsam { Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; Hl2.block( 2*i, 0, 2, 3 ) = Hli; - if (debug) { - std::cout << "Hxi= \n" << Hxi << std::endl; - std::cout << "Hxi.transpose() * Hxi= \n" << Hxi.transpose() * Hxi << std::endl; - std::cout << "Hxl.transpose() * Hxl= \n" << Hli.transpose() * Hli << std::endl; - } subInsert(b2,bi,2*i); } // Shur complement trick - Matrix H(6*keys_.size(), 6*keys_.size()); + Matrix H(6 * numKeys, 6 * numKeys); Matrix3 C2 = (Hl2.transpose() * Hl2).inverse(); - H = Hx2.transpose() * Hx2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * Hx2; + H = Hx2.transpose() * (Hx2 - (Hl2 * (C2 * (Hl2.transpose() * Hx2)))); - if (debug) { - std::cout << "Hx2" << "=[" << Hx2 << "];" << std::endl; - std::cout << "Hl2" << "=[" << Hl2 << "];" << std::endl; - std::cout << "H" << "=[" << H << "];" << std::endl; - - std::cout << "Cnoinv2"<< "=[" << Hl2.transpose() * Hl2 << "];" << std::endl; - std::cout << "C2"<< "=[" << C2 << "];" << std::endl; - std::cout << "================================================================================" << std::endl; - } - - Vector gs_vector = Hx2.transpose() * b2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * b2; + Vector gs_vector = Hx2.transpose() * (b2 - (Hl2 * (C2 * (Hl2.transpose() * b2)))); // Populate Gs and gs int GsCount2 = 0; - for(size_t i1 = 0; i1 < keys_.size(); i1++) { + for(size_t i1 = 0; i1 < numKeys; i1++) { gs.at(i1) = sub(gs_vector, 6*i1, 6*i1 + 6); - for(size_t i2 = 0; i2 < keys_.size(); i2++) { + for(size_t i2 = 0; i2 < numKeys; i2++) { if (i2 >= i1) { Gs.at(GsCount2) = H.block(6*i1, 6*i2, 6, 6); GsCount2++; @@ -368,27 +299,6 @@ namespace gtsam { } - if (debug) { - // Compare blockwise and full version - bool gs1_equal_gs = true; - for(size_t i = 0; i < measured_.size(); i++) { - std::cout << "gs.at(i) " << gs.at(i).transpose() << std::endl; - std::cout << "gs1.at(i) " << gs1.at(i).transpose() << std::endl; - std::cout << "gs.error " << (gs.at(i)- gs1.at(i)).transpose() << std::endl; - if( !equal(gs.at(i), gs1.at(i)), 1e-7) { - gs1_equal_gs = false; - } - } - std::cout << "gs1_equal_gs " << gs1_equal_gs << std::endl; - - for(size_t i = 0; i < keys_.size()*(keys_.size()+1)/2; i++) { - std::cout << "Gs.at(i) " << Gs.at(i).transpose() << std::endl; - std::cout << "Gs1.at(i) " << Gs1.at(i).transpose() << std::endl; - std::cout << "Gs.error " << (Gs.at(i)- Gs1.at(i)).transpose() << std::endl; - } - std::cout << "Gs1_equal_Gs " << gs1_equal_gs << std::endl; - } - // ========================================================================================================== return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); } @@ -400,7 +310,6 @@ namespace gtsam { * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. */ virtual double error(const Values& values) const { - bool debug = false; if (this->active(values)) { double overallError=0; @@ -415,39 +324,23 @@ namespace gtsam { } // We triangulate the 3D position of the landmark - if (debug) { - BOOST_FOREACH(const Pose3& pose, cameraPoses) { - std::cout << "Pose: " << pose << std::endl; - } - BOOST_FOREACH(const Point2& point, measured_) { - std::cout << "Point: " << point << std::endl; - } - } boost::optional point; - if (point_) { - point = point_; - std::cout << "Using existing point " << *point << std::endl; - } else { - //std::cout << "Triangulate during error calc" << std::endl; - point = triangulatePoint3(cameraPoses, measured_, *K_); + try { + point = triangulatePoint3(cameraPoses, measured_, *K_); + } catch( TriangulationCheiralityException& e) { + // point is behind one of the cameras, turn factor off by setting everything to 0 + //std::cout << e.what() << std::end; + return 0.0; } - if (debug) std::cout << "Result: " << *point << std::endl; - if(point) - { // triangulation produced a good estimate of landmark position + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_); - for(size_t i = 0; i < measured_.size(); i++) { - Pose3 pose = cameraPoses.at(i); - PinholeCamera camera(pose, *K_); - - Point2 reprojectionError(camera.project(*point) - measured_.at(i)); - overallError += noise_->distance( reprojectionError.vector() ); - } - 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; + Point2 reprojectionError(camera.project(*point) - measured_.at(i)); + overallError += noise_->distance( reprojectionError.vector() ); } + return overallError; } else { return 0.0; } @@ -458,6 +351,16 @@ namespace gtsam { return measured_; } + /** return the noise model */ + const SharedNoiseModel& noise() const { + return noise_; + } + + /** return the noise landmark */ + boost::optional point() const { + return point_; + } + /** return the calibration object */ inline const boost::shared_ptr calibration() const { return K_;