From 2e085ace91b3d00b9989818bcc4851ca19b77b8a Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Mon, 5 Aug 2013 19:25:08 +0000 Subject: [PATCH] Changes in SmartProjectionFactor --- .cproject | 390 ++++++++++---------- gtsam_unstable/slam/SmartProjectionFactor.h | 225 +++++++---- 2 files changed, 352 insertions(+), 263 deletions(-) diff --git a/.cproject b/.cproject index 24b5f8921..7257e427d 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -149,8 +147,16 @@ - - + + + + + + + + + + @@ -309,6 +315,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -335,7 +349,6 @@ make - tests/testBayesTree.run true false @@ -343,7 +356,6 @@ make - testBinaryBayesNet.run true false @@ -391,7 +403,6 @@ make - testSymbolicBayesNet.run true false @@ -399,7 +410,6 @@ make - tests/testSymbolicFactor.run true false @@ -407,7 +417,6 @@ make - testSymbolicFactorGraph.run true false @@ -423,20 +432,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -525,22 +525,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -557,6 +541,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -581,26 +581,34 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run true true true @@ -685,34 +693,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run + -j2 + clean true true true @@ -845,6 +845,22 @@ true true + + make + -j5 + testMultiProjectionFactor.run + true + true + true + + + make + -j5 + testSmartProjectionFactor.run + true + true + true + make -j5 @@ -933,6 +949,14 @@ true true + + make + -j5 + testTriangulation.run + true + true + true + make -j2 @@ -1079,7 +1103,6 @@ make - testGraph.run true false @@ -1087,7 +1110,6 @@ make - testJunctionTree.run true false @@ -1095,7 +1117,6 @@ make - testSymbolicBayesNetB.run true false @@ -1263,7 +1284,6 @@ make - testErrors.run true false @@ -1309,6 +1329,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1389,14 +1417,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1703,6 +1723,7 @@ make + testSimulated2DOriented.run true false @@ -1742,6 +1763,7 @@ make + testSimulated2D.run true false @@ -1749,6 +1771,7 @@ make + testSimulated3D.run true false @@ -1964,6 +1987,7 @@ make + tests/testGaussianISAM2 true false @@ -1985,6 +2009,102 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make -j3 @@ -2186,7 +2306,6 @@ cpack - -G DEB true false @@ -2194,7 +2313,6 @@ cpack - -G RPM true false @@ -2202,7 +2320,6 @@ cpack - -G TGZ true false @@ -2210,7 +2327,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2352,98 +2468,34 @@ true true - + make - -j2 - testRot3.run + -j5 + testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap true true true - + make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2487,38 +2539,6 @@ false true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 7b63b7814..de298e3bb 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -141,39 +141,39 @@ namespace gtsam { && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); } - /// Evaluate error h(x)-z and optionally derivatives - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const{ - - Vector a; - return a; - -// Point3 point = x.at(*keys_.end()); +// /// Evaluate error h(x)-z and optionally derivatives +// Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const{ // -// std::vector::iterator vit; -// for (vit = keys_.begin(); vit != keys_.end()-1; vit++) { -// Key key = (*vit); -// Pose3 pose = x.at(key); +// Vector a; +// return a; // -// if(body_P_sensor_) { -// if(H1) { -// gtsam::Matrix H0; -// PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); -// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -// *H1 = *H1 * H0; -// return reprojectionError.vector(); -// } else { -// PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); -// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -// return reprojectionError.vector(); -// } -// } else { -// PinholeCamera camera(pose, *K_); -// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -// return reprojectionError.vector(); -// } -// } - - } +//// Point3 point = x.at(*keys_.end()); +//// +//// std::vector::iterator vit; +//// for (vit = keys_.begin(); vit != keys_.end()-1; vit++) { +//// Key key = (*vit); +//// Pose3 pose = x.at(key); +//// +//// if(body_P_sensor_) { +//// if(H1) { +//// gtsam::Matrix H0; +//// PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); +//// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +//// *H1 = *H1 * H0; +//// return reprojectionError.vector(); +//// } else { +//// PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); +//// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +//// return reprojectionError.vector(); +//// } +//// } else { +//// PinholeCamera camera(pose, *K_); +//// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +//// return reprojectionError.vector(); +//// } +//// } +// +// } /// get the dimension of the factor (number of rows on linearization) virtual size_t dim() const { @@ -183,10 +183,6 @@ namespace gtsam { /// linearize returns a Hessianfactor that is an approximation of error(p) virtual boost::shared_ptr linearize(const Values& values, const Ordering& ordering) const { - std::vector Hx(keys_.size()); - std::vector Hl(keys_.size()); - std::vector b(keys_.size()); - // Collect all poses (Cameras) std::vector cameraPoses; @@ -196,66 +192,139 @@ namespace gtsam { else cameraPoses.push_back(values.at(k)); } - - // We triangulate the 3D position of the landmark + // We triangulate the 3D position of the landmark boost::optional point = triangulatePoint3(cameraPoses, measured_, *K_); - if(point){ - for(size_t i = 0; i < measured_.size(); i++) { - Pose3 pose = cameraPoses.at(i); - PinholeCamera camera(pose, *K_); - b.at(i) = ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector(); - } - } - else{ + if (!point) return HessianFactor::shared_ptr(new HessianFactor()); - } - // Allocate m^2 matrix blocks - std::vector< std::vector > Hxl(keys_.size(), std::vector( keys_.size())); - - // Allocate inv(Hl'Hl) - Matrix3 C; - for(size_t i1 = 0; i1 < keys_.size(); i1++) { - C += Hl.at(i1).transpose() * Hl.at(i1); - } - C = C.inverse(); + std::cout << "point " << *point << std::endl; + std::vector Gs(keys_.size()*(keys_.size()+1)/2); + std::vector gs(keys_.size()); + double f = 0; // fill in the keys std::vector js; BOOST_FOREACH(const Key& k, keys_) { js += ordering[k]; } - // Calculate sub blocks - for(size_t i1 = 0; i1 < keys_.size(); i1++) { - for(size_t i2 = 0; i2 < keys_.size(); i2++) { - Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose(); + bool blockwise = false; + +// { + // ========================================================================================================== + std::vector Hx(keys_.size()); + std::vector Hl(keys_.size()); + std::vector b(keys_.size()); + + 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(); } - } - // Shur complement trick + // Shur complement trick - // Populate Gs and gs - std::vector Gs(keys_.size()*(keys_.size()+1)/2); - std::vector gs(keys_.size()); - double f = 0; - int GsCount = 0; - for(size_t i1 = 0; i1 < keys_.size(); i1++) { - gs.at(i1) = Hx.at(i1).transpose() * b.at(i1); + // Allocate m^2 matrix blocks + std::vector< std::vector > Hxl(keys_.size(), std::vector( keys_.size())); - for(size_t i2 = 0; i2 < keys_.size(); i2++) { - gs.at(i1) += Hxl[i1][i2] * b.at(i2); + // Allocate inv(Hl'Hl) + Matrix3 C; + for(size_t i1 = 0; i1 < keys_.size(); i1++) { + C += Hl.at(i1).transpose() * Hl.at(i1); + } + C = C.inverse(); - if (i2 >= i1) { - Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2); - GsCount++; + // Calculate sub blocks + for(size_t i1 = 0; i1 < keys_.size(); i1++) { + for(size_t i2 = 0; i2 < keys_.size(); i2++) { + Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose(); } } + // Populate Gs and gs + int GsCount = 0; + for(size_t i1 = 0; i1 < keys_.size(); i1++) { + gs.at(i1) = Hx.at(i1).transpose() * b.at(i1); + + for(size_t i2 = 0; i2 < keys_.size(); 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); + GsCount++; + } + } + } +// } + + // debug only + std::vector Gs2(keys_.size()*(keys_.size()+1)/2); + std::vector gs2(keys_.size()); + +// { // 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()); + + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_); + Matrix Hxi, Hli; + Vector bi = ( camera.project(*point,Hxi,Hli) - measured_.at(i) ).vector(); + Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; + Hl2.block( 2*i, 0, 2, 3 ) = Hli; + subInsert(b2,bi,2*i); + + std::cout << "Hx " << Hx2 << std::endl; + std::cout << "Hl " << Hl2 << std::endl; + std::cout << "b " << b2.transpose() << std::endl; + std::cout << "Hxi - Hx.at(i) " << Hxi - Hx.at(i) << std::endl; + std::cout << "Hli - Hl.at(i) " << Hli - Hl.at(i) << std::endl; + } + + // Shur complement trick + Matrix H(6*keys_.size(), 6*keys_.size()); + Matrix3 C2 = (Hl2.transpose() * Hl2).inverse(); + H = Hx2.transpose() * Hx2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * Hx2; + Vector gs2_vector = Hx2.transpose() * b2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * b2; + + std::cout << "C - C2 " << C - C2 << std::endl; + + // Populate Gs and gs + int GsCount2 = 0; + for(size_t i1 = 0; i1 < keys_.size(); i1++) { + gs2.at(i1) = sub(gs2_vector, 6*i1, 6*i1 + 6); + + for(size_t i2 = 0; i2 < keys_.size(); i2++) { + if (i2 >= i1) { + Gs2.at(GsCount2) = H.block(6*i1, 6*i2, 6, 6); + GsCount2++; + } + } + } +// } + + // Compare blockwise and full version + bool gs2_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 << "gs2.at(i) " << gs2.at(i).transpose() << std::endl; + std::cout << "gs.error " << (gs.at(i)- gs2.at(i)).transpose() << std::endl; + if( !equal(gs.at(i), gs2.at(i)), 1e-7) { + gs2_equal_gs = false; + } } - return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); + std::cout << "gs2_equal_gs " << gs2_equal_gs << std::endl; + + + // ========================================================================================================== + return HessianFactor::shared_ptr(new HessianFactor(js, Gs2, gs2, f)); } /** @@ -284,17 +353,17 @@ namespace gtsam { if(point) { // triangulation produced a good estimate of landmark position - std::cout << "point " << *point << std::endl; +// std::cout << "point " << *point << std::endl; for(size_t i = 0; i < measured_.size(); i++) { Pose3 pose = cameraPoses.at(i); PinholeCamera camera(pose, *K_); - std::cout << "pose.compose(*body_P_sensor_) " << pose << std::endl; +// std::cout << "pose.compose(*body_P_sensor_) " << pose << std::endl; Point2 reprojectionError(camera.project(*point) - measured_.at(i)); - std::cout << "reprojectionError " << reprojectionError << std::endl; +// std::cout << "reprojectionError " << reprojectionError << std::endl; overallError += noise_->distance( reprojectionError.vector() ); - std::cout << "noise_->distance( reprojectionError.vector() ) " << noise_->distance( reprojectionError.vector() ) << std::endl; +// std::cout << "noise_->distance( reprojectionError.vector() ) " << noise_->distance( reprojectionError.vector() ) << std::endl; } return sqrt(overallError); }else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error