From 4123053c8164d236fb6a25a182abd5a34814ed25 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 8 Apr 2014 18:55:42 -0400 Subject: [PATCH 01/70] progress on grouping - still seg faults --- gtsam_unstable/slam/SmartFactorBase.h | 42 ++++++++++++++++++--------- 1 file changed, 29 insertions(+), 13 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index ebef221bb..973341477 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -417,7 +417,7 @@ public: } // **************************************************************************************************** - boost::shared_ptr > updateAugmentedHessian( + void updateAugmentedHessian( const Cameras& cameras, const Point3& point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian) const { @@ -435,8 +435,8 @@ public: dims.back() = 1; updateSparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - std::cout << "f "<< f <keys_.size(); // cameras observing current point - size_t aug_numKeys = augmentedHessian.rows() - 1; // all cameras in the group + size_t aug_numKeys = (augmentedHessian.rows() - 1)/D; // all cameras in the group + +// MatrixDD delta = eye(D); +// size_t n1 = numKeys+1; +// for (size_t i1=0; i1 < n1-1; i1++){ +// MatrixDD Z1 = augmentedHessian(i1,i1).selfadjointView(); +// std::cout << i1 << " " << "\n" << Z1 << std::endl; +// augmentedHessian(i1,i1) = Z1 + delta; +// MatrixDD Z2 = augmentedHessian(i1,i1).selfadjointView(); +// std::cout << i1 << " " << "\n" << Z2 << std::endl; +//// for (size_t i2=i1+1; i2 < n1-1; i2++){ +//// Z = augmentedHessian(i1,i2).knownOffDiagonal(); // + delta; +//// std::cout << i1 << " " << i2 << "\n" << Z << std::endl; +//// } +// } + + MatrixDD matrixBlock; + VectorD vectorBlock; // Blockwise Schur complement for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera @@ -497,27 +514,26 @@ public: // D = (Dx2) * (2) // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b size_t aug_i1 = this->keys_[i1]; - std::cout << "i1 "<< i1 < (2 * i1) // F' * b - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) - std::cout << "filled 1 " <(2 * i1, 0).transpose() * Fi1); // upper triangular part of the hessian for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera const Matrix2D& Fi2 = Fblocks.at(i2).second; size_t aug_i2 = this->keys_[i2]; - std::cout << "i2 "<< i2 <(2 * i2, 0).transpose() * Fi2); } } // end of for over cameras From d8fafce2245370697b822bae490031371efbde47 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 9 Apr 2014 21:00:38 -0400 Subject: [PATCH 02/70] minor changes to cout when verbosity is TRYLAMBDA --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 6be69e710..0bf06f21f 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -256,6 +256,9 @@ void LevenbergMarquardtOptimizer::iterate() { double newlinearizedError = linear->error(delta); double linearizedCostChange = state_.error - newlinearizedError; + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "newlinearizedError = " << newlinearizedError << + "linearizedCostChange = " << linearizedCostChange << endl; if (linearizedCostChange >= 0) { // step is valid // update values @@ -266,10 +269,14 @@ void LevenbergMarquardtOptimizer::iterate() { // compute new error gttic(compute_error); if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "calculating error" << endl; + cout << "calculating error:" << endl; newError = graph_.error(newValues); gttoc(compute_error); + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "old error (" << state_.error + << ") new (tentative) error (" << newError << ")" << endl; + // cost change in the original, nonlinear system (old - new) double costChange = state_.error - newError; @@ -297,8 +304,7 @@ void LevenbergMarquardtOptimizer::iterate() { break; } else if (!stopSearchingLambda) { // we failed to solved the system or we had no decrease in cost if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "increasing lambda: old error (" << state_.error - << ") new error (" << newError << ")" << endl; + cout << "increasing lambda" << endl; increaseLambda(); // check if lambda is too big @@ -310,6 +316,8 @@ void LevenbergMarquardtOptimizer::iterate() { break; } } else { // the change in the cost is very small and it is not worth trying bigger lambdas + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl; break; } } // end while From dc7b5d58c025c0f089b1d7ef93ae76dc9419d9c1 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 9 Apr 2014 21:01:03 -0400 Subject: [PATCH 03/70] implemented efficient update of Hessian matrix via Schur complement --- gtsam_unstable/slam/SmartFactorBase.h | 266 ++++++++++++++------------ 1 file changed, 147 insertions(+), 119 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index 973341477..49242d5ac 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -192,7 +192,7 @@ public: b[2 * i + 1] = e.y(); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } i += 1; } @@ -219,10 +219,10 @@ public: try { Point2 reprojectionError(camera.project(point) - zi); overallError += 0.5 - * this->noise_.at(i)->distance(reprojectionError.vector()); + * this->noise_.at(i)->distance(reprojectionError.vector()); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } i += 1; } @@ -244,7 +244,7 @@ public: cameras[i].project(point, boost::none, Ei); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } this->noise_.at(i)->WhitenSystem(Ei, b); E.block<2, 3>(2 * i, 0) = Ei; @@ -274,7 +274,7 @@ public: -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); @@ -322,7 +322,7 @@ public: const double lambda = 0.0) const { int numKeys = this->keys_.size(); - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); F = zeros(2 * numKeys, D * numKeys); @@ -345,7 +345,7 @@ public: diagonalDamping); // diagonalDamping should have no effect (only on PointCov) // Do SVD on A - Eigen::JacobiSVD < Matrix > svd(E, Eigen::ComputeFullU); + Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); // Enull = zeros(2 * numKeys, 2 * numKeys - 3); int numKeys = this->keys_.size(); @@ -361,7 +361,7 @@ public: const Cameras& cameras, const Point3& point) const { int numKeys = this->keys_.size(); - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); F.resize(2 * numKeys, D * numKeys); F.setZero(); @@ -380,14 +380,14 @@ public: int numKeys = this->keys_.size(); - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; Matrix E; Matrix3 PointCov; Vector b; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); -//#define HESSIAN_BLOCKS +//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix #ifdef HESSIAN_BLOCKS // Create structures for Hessian Factors std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); @@ -400,46 +400,23 @@ public: //std::vector < Vector > gs2(gs.begin(), gs.end()); return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, f); -#else + > (this->keys_, Gs, gs, f); +#else // we create directly a SymmetricBlockMatrix size_t n1 = D * numKeys + 1; std::vector dims(numKeys + 1); // this also includes the b term std::fill(dims.begin(), dims.end() - 1, D); dims.back() = 1; - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - augmentedHessian(numKeys,numKeys)(0,0) = f; - return boost::make_shared >( - this->keys_, augmentedHessian); - + augmentedHessian(numKeys, numKeys)(0, 0) = f; + return boost::make_shared >(this->keys_, + augmentedHessian); #endif } // **************************************************************************************************** - void updateAugmentedHessian( - const Cameras& cameras, const Point3& point, const double lambda, - bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian) const { - - int numKeys = this->keys_.size(); - - std::vector < KeyMatrix2D > Fblocks; - Matrix E; - Matrix3 PointCov; - Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - - std::vector dims(numKeys + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, D); - dims.back() = 1; - - updateSparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - // std::cout << "f "<< f <& Fblocks, const Matrix& E, const Matrix& PointCov, const Vector& b, /*output ->*/std::vector& Gs, std::vector& gs) const { @@ -466,7 +443,7 @@ public: int GsCount2 = 0; for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment < D > (i1D); + gs.at(i1) = gs_vector.segment(i1D); for (DenseIndex i2 = 0; i2 < numKeys; i2++) { if (i2 >= i1) { Gs.at(GsCount2) = H.block(i1D, i2 * D); @@ -476,69 +453,6 @@ public: } } - // **************************************************************************************************** - void updateSparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); // cameras observing current point - size_t aug_numKeys = (augmentedHessian.rows() - 1)/D; // all cameras in the group - -// MatrixDD delta = eye(D); -// size_t n1 = numKeys+1; -// for (size_t i1=0; i1 < n1-1; i1++){ -// MatrixDD Z1 = augmentedHessian(i1,i1).selfadjointView(); -// std::cout << i1 << " " << "\n" << Z1 << std::endl; -// augmentedHessian(i1,i1) = Z1 + delta; -// MatrixDD Z2 = augmentedHessian(i1,i1).selfadjointView(); -// std::cout << i1 << " " << "\n" << Z2 << std::endl; -//// for (size_t i2=i1+1; i2 < n1-1; i2++){ -//// Z = augmentedHessian(i1,i2).knownOffDiagonal(); // + delta; -//// std::cout << i1 << " " << i2 << "\n" << Z << std::endl; -//// } -// } - - MatrixDD matrixBlock; - VectorD vectorBlock; - - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera - - const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; - - // D = (Dx2) * (2) - // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - size_t aug_i1 = this->keys_[i1]; - - // augmentedHessian(aug_i1,aug_numKeys) - vectorBlock = augmentedHessian(aug_i1,aug_numKeys).knownOffDiagonal(); - augmentedHessian(aug_i1,aug_numKeys) = vectorBlock + - Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) - - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) - matrixBlock = augmentedHessian(aug_i1,aug_i1).selfadjointView(); - augmentedHessian(aug_i1,aug_i1) = matrixBlock+ - Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); - - // upper triangular part of the hessian - for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - size_t aug_i2 = this->keys_[i2]; - - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); - augmentedHessian(aug_i1, aug_i2) = matrixBlock - - Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); - } - } // end of for over cameras - } - // **************************************************************************************************** void sparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, @@ -558,20 +472,20 @@ public: // D = (Dx2) * (2) // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - augmentedHessian(i1,numKeys) = Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) - augmentedHessian(i1,i1) = - Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + augmentedHessian(i1, i1) = Fi1.transpose() + * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); // upper triangular part of the hessian - for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera + for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera const Matrix2D& Fi2 = Fblocks.at(i2).second; // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian(i1,i2) = - -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + augmentedHessian(i1, i2) = -Fi1.transpose() + * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); } } // end of for over cameras } @@ -602,24 +516,138 @@ public: { // for i1 = i2 // D = (Dx2) * (2) - gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) - gs.at(i1) -= Fi1.transpose() * (Ei1_P * (E.transpose() * b)); + gs.at(i1) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b + -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) - Gs.at(GsIndex) = Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + Gs.at(GsIndex) = Fi1.transpose() + * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); GsIndex++; } // upper triangular part of the hessian - for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera + for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera const Matrix2D& Fi2 = Fblocks.at(i2).second; // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - Gs.at(GsIndex) = -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + Gs.at(GsIndex) = -Fi1.transpose() + * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); GsIndex++; } } // end of for over cameras } + + // **************************************************************************************************** + void updateAugmentedHessian(const Cameras& cameras, const Point3& point, + const double lambda, bool diagonalDamping, + SymmetricBlockMatrix& augmentedHessian, + const FastVector allKeys) const { + + // int numKeys = this->keys_.size(); + + std::vector Fblocks; + Matrix E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); + + updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + } + + // **************************************************************************************************** + void updateSparseSchurComplement(const std::vector& Fblocks, + const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const double f, const FastVector allKeys, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { + // Schur complement trick + // Gs = F' * F - F' * E * P * E' * F + // gs = F' * (b - E * P * E' * b) + MatrixDD matrixBlock; + VectorD vectorBlock; + + FastMap KeySlotMap; + for (size_t slot=0; slot < allKeys.size(); slot++) + KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); + + bool debug= false; + + // a single point is observed in numKeys cameras + size_t numKeys = this->keys_.size(); // cameras observing current point + size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group + + // Blockwise Schur complement + for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor + + const Matrix2D& Fi1 = Fblocks.at(i1).second; + const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + + // D = (Dx2) * (2) + // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) + // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) + Key cameraKey_i1 = this->keys_[i1]; + size_t aug_i1 = KeySlotMap[cameraKey_i1]; + + // information vector - store previous vector + vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); + if(debug) std::cout << "(before) augmentedHessian(" << aug_i1 << "," << aug_numKeys << ")= \n" << + vectorBlock << std::endl; + + // add contribution of current factor + augmentedHessian(aug_i1, aug_numKeys) = vectorBlock + + Fi1.transpose() * b.segment<2>(2 * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + + vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); + if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_numKeys << ")= \n" << + vectorBlock << std::endl; + + // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // main block diagonal - store previous block + matrixBlock = augmentedHessian(aug_i1, aug_i1); + if(debug) std::cout << "(before) augmentedHessian(" << aug_i1 << "," << aug_i1 << ")= \n" << + matrixBlock << std::endl; + + // add contribution of current factor + augmentedHessian(aug_i1, aug_i1) = matrixBlock + + Fi1.transpose() + * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + + matrixBlock = augmentedHessian(aug_i1, aug_i1); + if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_i1 << ")= \n" << + matrixBlock << std::endl; + + // upper triangular part of the hessian + for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera + const Matrix2D& Fi2 = Fblocks.at(i2).second; + + Key cameraKey_i2 = this->keys_[i2]; + size_t aug_i2 = KeySlotMap[cameraKey_i2]; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + // off diagonal block - store previous block + matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); + + if(debug) std::cout << "(aug_i1= " << aug_i1 << ", aug_i2= " << aug_i2 << ") (i2= " <(2 * i2, 0).transpose() * Fi2); + + if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_i2 << ")= \n" << + augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() << std::endl; + + matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); + if(debug) std::cout << "(after, after) augmentedHessian(" << aug_i1 << "," << aug_i2 << ")= \n" << + matrixBlock<< std::endl; + } + } // end of for over cameras + + augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f; + } + // **************************************************************************************************** boost::shared_ptr > createImplicitSchurFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, @@ -636,13 +664,13 @@ public: boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector < KeyMatrix2D > Fblocks; + std::vector Fblocks; Matrix E; Matrix3 PointCov; Vector b; computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); - return boost::make_shared < JacobianFactorQ > (Fblocks, E, PointCov, b); + return boost::make_shared >(Fblocks, E, PointCov, b); } private: From 4bdefc3f702c2182d7d0f96e734b940463b881dd Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 9 Apr 2014 21:04:52 -0400 Subject: [PATCH 04/70] removed debug bloks --- gtsam_unstable/slam/SmartFactorBase.h | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index 49242d5ac..40b8b2b09 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -597,10 +597,6 @@ public: + Fi1.transpose() * b.segment<2>(2 * i1) // F' * b - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) - vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); - if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_numKeys << ")= \n" << - vectorBlock << std::endl; - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) // main block diagonal - store previous block matrixBlock = augmentedHessian(aug_i1, aug_i1); @@ -612,10 +608,6 @@ public: + Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); - matrixBlock = augmentedHessian(aug_i1, aug_i1); - if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_i1 << ")= \n" << - matrixBlock << std::endl; - // upper triangular part of the hessian for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera const Matrix2D& Fi2 = Fblocks.at(i2).second; @@ -635,13 +627,6 @@ public: augmentedHessian(aug_i1, aug_i2) = matrixBlock - Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); - - if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_i2 << ")= \n" << - augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() << std::endl; - - matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); - if(debug) std::cout << "(after, after) augmentedHessian(" << aug_i1 << "," << aug_i2 << ")= \n" << - matrixBlock<< std::endl; } } // end of for over cameras From 6ef11bb2975ffe3d3c18c484aeb5ddc0e055fcfa Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 9 Apr 2014 21:14:38 -0400 Subject: [PATCH 05/70] removed debug statements --- gtsam_unstable/slam/SmartFactorBase.h | 23 +++++------------------ 1 file changed, 5 insertions(+), 18 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index 40b8b2b09..d4d3fd489 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -569,8 +569,6 @@ public: for (size_t slot=0; slot < allKeys.size(); slot++) KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); - bool debug= false; - // a single point is observed in numKeys cameras size_t numKeys = this->keys_.size(); // cameras observing current point size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group @@ -588,21 +586,15 @@ public: size_t aug_i1 = KeySlotMap[cameraKey_i1]; // information vector - store previous vector - vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); - if(debug) std::cout << "(before) augmentedHessian(" << aug_i1 << "," << aug_numKeys << ")= \n" << - vectorBlock << std::endl; - + // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); // add contribution of current factor - augmentedHessian(aug_i1, aug_numKeys) = vectorBlock + augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() + Fi1.transpose() * b.segment<2>(2 * i1) // F' * b - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) // main block diagonal - store previous block matrixBlock = augmentedHessian(aug_i1, aug_i1); - if(debug) std::cout << "(before) augmentedHessian(" << aug_i1 << "," << aug_i1 << ")= \n" << - matrixBlock << std::endl; - // add contribution of current factor augmentedHessian(aug_i1, aug_i1) = matrixBlock + Fi1.transpose() @@ -613,18 +605,13 @@ public: const Matrix2D& Fi2 = Fblocks.at(i2).second; Key cameraKey_i2 = this->keys_[i2]; - size_t aug_i2 = KeySlotMap[cameraKey_i2]; + size_t aug_i2 = KeySlotMap[cameraKey_i2]; // (DxD) = (Dx2) * ( (2x2) * (2xD) ) // off diagonal block - store previous block - matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); - - if(debug) std::cout << "(aug_i1= " << aug_i1 << ", aug_i2= " << aug_i2 << ") (i2= " <(2 * i2, 0).transpose() * Fi2); } From 926b4c644316833067df7d1b1bf5ec5b0ff2b8da Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 14 Apr 2014 11:41:55 -0400 Subject: [PATCH 06/70] removed temporary variables - minor --- gtsam_unstable/slam/SmartFactorBase.h | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index d4d3fd489..2852e9d83 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -562,8 +562,9 @@ public: // Schur complement trick // Gs = F' * F - F' * E * P * E' * F // gs = F' * (b - E * P * E' * b) + MatrixDD matrixBlock; - VectorD vectorBlock; + typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix FastMap KeySlotMap; for (size_t slot=0; slot < allKeys.size(); slot++) @@ -582,8 +583,8 @@ public: // D = (Dx2) * (2) // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) - Key cameraKey_i1 = this->keys_[i1]; - size_t aug_i1 = KeySlotMap[cameraKey_i1]; + // Key cameraKey_i1 = this->keys_[i1]; + DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]]; // information vector - store previous vector // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); @@ -596,24 +597,22 @@ public: // main block diagonal - store previous block matrixBlock = augmentedHessian(aug_i1, aug_i1); // add contribution of current factor - augmentedHessian(aug_i1, aug_i1) = matrixBlock - + Fi1.transpose() - * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + augmentedHessian(aug_i1, aug_i1) = matrixBlock + + ( Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1) ); // upper triangular part of the hessian for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera const Matrix2D& Fi2 = Fblocks.at(i2).second; - Key cameraKey_i2 = this->keys_[i2]; - size_t aug_i2 = KeySlotMap[cameraKey_i2]; + //Key cameraKey_i2 = this->keys_[i2]; + DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; // (DxD) = (Dx2) * ( (2x2) * (2xD) ) // off diagonal block - store previous block // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); // add contribution of current factor augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() - - Fi1.transpose() - * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + - Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); } } // end of for over cameras From 4552327e54e0bfd1d20cb1479ef844b506b335be Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 16 Apr 2014 19:41:23 -0400 Subject: [PATCH 07/70] added space :) --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 0bf06f21f..3f7e80ed4 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -258,7 +258,7 @@ void LevenbergMarquardtOptimizer::iterate() { double linearizedCostChange = state_.error - newlinearizedError; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "newlinearizedError = " << newlinearizedError << - "linearizedCostChange = " << linearizedCostChange << endl; + " linearizedCostChange = " << linearizedCostChange << endl; if (linearizedCostChange >= 0) { // step is valid // update values @@ -280,7 +280,7 @@ void LevenbergMarquardtOptimizer::iterate() { // cost change in the original, nonlinear system (old - new) double costChange = state_.error - newError; - if (linearizedCostChange > 1e-15) { // the error has to decrease to satify this condition + if (linearizedCostChange > 1e-15) { // the error has to decrease to satisfy this condition // fidelity of linearized model VS original system between modelFidelity = costChange / linearizedCostChange; // if we decrease the error in the nonlinear system and modelFidelity is above threshold @@ -293,7 +293,6 @@ void LevenbergMarquardtOptimizer::iterate() { // if the change is small we terminate if (fabs(costChange) < minAbsoluteTolerance) stopSearchingLambda = true; - } } From dd780e356cf9aadb15edb2b91832e87972ded919 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 21 Apr 2014 18:59:33 -0400 Subject: [PATCH 08/70] fixed computation of linearized error in implicit schur factors --- gtsam_unstable/slam/ImplicitSchurFactor.h | 69 +++++++++++++++++++---- 1 file changed, 59 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam_unstable/slam/ImplicitSchurFactor.h index 234d13f1f..d7d1bc21c 100644 --- a/gtsam_unstable/slam/ImplicitSchurFactor.h +++ b/gtsam_unstable/slam/ImplicitSchurFactor.h @@ -235,25 +235,33 @@ public: typedef std::vector Error2s; /** - * @brief Calculate corrected error Q*e = (I - E*P*E')*e + * @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b) */ - void projectError(const Error2s& e1, Error2s& e2) const { + void projectError2(const Error2s& e1, Error2s& e2) const { - // d1 = E.transpose() * e1 = (3*2m)*2m + // d1 = E.transpose() * (e1-2*b) = (3*2m)*2m Vector3 d1; d1.setZero(); for (size_t k = 0; k < size(); k++) - d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; + d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * (e1[k] - 2 * b_.segment < 2 > (k * 2)); // d2 = E.transpose() * e1 = (3*2m)*2m Vector3 d2 = PointCovariance_ * d1; // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; + e2[k] = e1[k] - 2 * b_.segment < 2 > (k * 2) - E_.block < 2, 3 > (2 * k, 0) * d2; } - /// needed to be GaussianFactor - (I - E*P*E')*(F*x - b) + /* + * This definition matches the linearized error in the Hessian Factor: + * LinError(x) = x'*H*x - 2*x'*eta + f + * with: + * H = F' * (I-E'*P*E) * F = F' * Q * F + * eta = F' * (I-E'*P*E) * b = F' * Q * b + * f = nonlinear error + * (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f + */ virtual double error(const VectorValues& x) const { // resize does not do malloc if correct size @@ -262,15 +270,56 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); - projectError(e1, e2); + e1[k] = Fblocks_[k].second * x.at(keys_[k]); + projectError2(e1, e2); double result = 0; for (size_t k = 0; k < size(); ++k) - result += dot(e2[k], e2[k]); - return 0.5 * result; + result += dot(e1[k], e2[k]); + + double f = b_.squaredNorm(); + return 0.5 * (result + f); } + /// needed to be GaussianFactor - (I - E*P*E')*(F*x - b) + // This is wrong and does not match the definition in Hessian + // virtual double error(const VectorValues& x) const { + // + // // resize does not do malloc if correct size + // e1.resize(size()); + // e2.resize(size()); + // + // // e1 = F * x - b = (2m*dm)*dm + // for (size_t k = 0; k < size(); ++k) + // e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); + // projectError(e1, e2); + // + // double result = 0; + // for (size_t k = 0; k < size(); ++k) + // result += dot(e2[k], e2[k]); + // + // std::cout << "implicitFactor::error result " << result << std::endl; + // return 0.5 * result; + // } + /** + * @brief Calculate corrected error Q*e = (I - E*P*E')*e + */ + void projectError(const Error2s& e1, Error2s& e2) const { + + // d1 = E.transpose() * e1 = (3*2m)*2m + Vector3 d1; + d1.setZero(); + for (size_t k = 0; k < size(); k++) + d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; + + // d2 = E.transpose() * e1 = (3*2m)*2m + Vector3 d2 = PointCovariance_ * d1; + + // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] + for (size_t k = 0; k < size(); k++) + e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; + } + /// Scratch space for multiplyHessianAdd mutable Error2s e1, e2; From e98c4c2ac2ec6e4cb8ef359e508d538e11d75f3b Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 21 Apr 2014 19:18:21 -0400 Subject: [PATCH 09/70] added output message for TRYLAMBDA verbosity level --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 3f7e80ed4..827df7619 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -291,8 +291,12 @@ void LevenbergMarquardtOptimizer::iterate() { double minAbsoluteTolerance = params_.relativeErrorTol * state_.error; // if the change is small we terminate - if (fabs(costChange) < minAbsoluteTolerance) + if (fabs(costChange) < minAbsoluteTolerance){ + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "fabs(costChange)="< Date: Tue, 22 Apr 2014 11:55:16 -0400 Subject: [PATCH 10/70] moved logging at the end of the iteration --- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 827df7619..e6c24f050 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -214,20 +214,6 @@ void LevenbergMarquardtOptimizer::iterate() { // Build damped system for this lambda (adds prior factors that make it like gradient descent) GaussianFactorGraph dampedSystem = buildDampedSystem(*linear); - // Log current error/lambda to file - if (!params_.logFile.empty()) { - ofstream os(params_.logFile.c_str(), ios::app); - - boost::posix_time::ptime currentTime = - boost::posix_time::microsec_clock::universal_time(); - - os << state_.totalNumberInnerIterations << "," - << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," - << state_.error << "," << state_.lambda << endl; - } - - ++state_.totalNumberInnerIterations; - // Try solving double modelFidelity = 0.0; bool step_is_successful = false; @@ -300,6 +286,17 @@ void LevenbergMarquardtOptimizer::iterate() { } } + // Log current error/lambda to file + if (!params_.logFile.empty()) { + ofstream os(params_.logFile.c_str(), ios::app); + boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time(); + os << /*inner iterations*/ state_.totalNumberInnerIterations << "," + << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," + << /*current error*/ state_.error << "," << state_.lambda << "," + << /*outer iterations*/ state_.iterations << endl; + } + ++state_.totalNumberInnerIterations; + if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity state_.values.swap(newValues); state_.error = newError; From 37b750411f14d2f1a547fc97f201da0e28615ced Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 22 Apr 2014 18:38:12 -0400 Subject: [PATCH 11/70] added gradientAtZero with raw memory access --- gtsam/linear/GaussianFactor.h | 3 +++ gtsam/linear/GaussianFactorGraph.h | 2 +- gtsam/linear/HessianFactor.cpp | 17 +++++++++++++++++ gtsam/linear/HessianFactor.h | 2 ++ gtsam/linear/JacobianFactor.cpp | 5 +++++ gtsam/linear/JacobianFactor.h | 3 +++ gtsam_unstable/slam/ImplicitSchurFactor.h | 22 ++++++++++++++++++++++ 7 files changed, 53 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 543822ce0..8c8e2cb2b 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -130,6 +130,9 @@ namespace gtsam { /// A'*b for Jacobian, eta for Hessian virtual VectorValues gradientAtZero() const = 0; + /// A'*b for Jacobian, eta for Hessian (raw memory version) + virtual void gradientAtZero(double* d) const = 0; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 50442ac6b..ae5730b68 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -257,7 +257,7 @@ namespace gtsam { * @param [output] g A VectorValues to store the gradient, which must be preallocated, * see allocateVectorValues * @return The gradient as a VectorValues */ - VectorValues gradientAtZero() const; + virtual VectorValues gradientAtZero() const; /** Optimize along the gradient direction, with a closed-form computation to perform the line * search. The gradient is computed about \f$ \delta x=0 \f$. diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 18fa44e8b..8d76c7651 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -600,6 +600,23 @@ VectorValues HessianFactor::gradientAtZero() const { return g; } +/* ************************************************************************* */ +// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n +void HessianFactor::gradientAtZero(double* d) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + DVector dj = -info_(pos,size()).knownOffDiagonal(); + DMap(d + 9 * j) += dj; + } +} + /* ************************************************************************* */ std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index faf2ccd33..62d84925c 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -387,6 +387,8 @@ namespace gtsam { /// eta for Hessian VectorValues gradientAtZero() const; + virtual void gradientAtZero(double* d) const; + /** * Densely partially eliminate with Cholesky factorization. JacobianFactors are * left-multiplied with their transpose to form the Hessian using the conversion constructor diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index c080f75cb..bb9d5afc0 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -573,6 +573,11 @@ VectorValues JacobianFactor::gradientAtZero() const { return g; } +/* ************************************************************************* */ +void JacobianFactor::gradientAtZero(double* d) const { + throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); +} + /* ************************************************************************* */ pair JacobianFactor::jacobian() const { pair result = jacobianUnweighted(); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 5a567c2c7..b90012822 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -286,6 +286,9 @@ namespace gtsam { /// A'*b for Jacobian VectorValues gradientAtZero() const; + /* ************************************************************************* */ + virtual void gradientAtZero(double* d) const; + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam_unstable/slam/ImplicitSchurFactor.h index d7d1bc21c..8237b8c07 100644 --- a/gtsam_unstable/slam/ImplicitSchurFactor.h +++ b/gtsam_unstable/slam/ImplicitSchurFactor.h @@ -426,6 +426,28 @@ public: return g; } + /** + * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS + */ + void gradientAtZero(double* d) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // calculate Q*b + e1.resize(size()); + e2.resize(size()); + for (size_t k = 0; k < size(); k++) + e1[k] = b_.segment < 2 > (2 * k); + projectError(e1, e2); + + for (size_t k = 0; k < size(); ++k) { // for each camera in the factor + Key j = keys_[k]; + DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k]; + } + } + }; // ImplicitSchurFactor From e56666e85c651ba7ba645fec11e105f7d6686b5d Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 23 Apr 2014 14:53:01 -0400 Subject: [PATCH 12/70] fixed print function --- gtsam_unstable/slam/ImplicitSchurFactor.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam_unstable/slam/ImplicitSchurFactor.h index 8237b8c07..0afdbcb71 100644 --- a/gtsam_unstable/slam/ImplicitSchurFactor.h +++ b/gtsam_unstable/slam/ImplicitSchurFactor.h @@ -87,7 +87,8 @@ public: } /// print - void print(const std::string& s, const KeyFormatter& formatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << " ImplicitSchurFactor " << std::endl; Factor::print(s); std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; From 2b52bad56950e4ae2966a503d9cba3c0e3a7a0e0 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 23 Apr 2014 15:15:48 -0400 Subject: [PATCH 13/70] optimized point damping --- gtsam_unstable/slam/SmartFactorBase.h | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index 2852e9d83..79a56ea7f 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -302,15 +302,18 @@ public: // Point covariance inv(E'*E) Matrix3 EtE = E.transpose() * E; - Matrix3 DMatrix = eye(E.cols()); // damping matrix if (diagonalDamping) { // diagonal of the hessian - DMatrix(0, 0) = EtE(0, 0); - DMatrix(1, 1) = EtE(1, 1); - DMatrix(2, 2) = EtE(2, 2); + EtE(0, 0) += lambda * EtE(0, 0); + EtE(1, 1) += lambda * EtE(1, 1); + EtE(2, 2) += lambda * EtE(2, 2); + }else{ + EtE(0, 0) += lambda; + EtE(1, 1) += lambda; + EtE(2, 2) += lambda; } - PointCov.noalias() = (EtE + lambda * DMatrix).inverse(); + PointCov.noalias() = (EtE).inverse(); return f; } From d9e93f316ada77e687d0853f73332fc4c2e8de2e Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 23 Apr 2014 18:43:14 -0400 Subject: [PATCH 14/70] error computation now does not include noise model --- gtsam_unstable/slam/SmartFactorBase.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index 79a56ea7f..e60b7e930 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -218,8 +218,7 @@ public: const Point2& zi = this->measured_.at(i); try { Point2 reprojectionError(camera.project(point) - zi); - overallError += 0.5 - * this->noise_.at(i)->distance(reprojectionError.vector()); + overallError += 0.5 * reprojectionError.vector().squaredNorm(); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; exit(EXIT_FAILURE); From 8c657f88579b1df333ca7fff0e29f4ff8ee83375 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 23 Apr 2014 19:00:15 -0400 Subject: [PATCH 15/70] slight change in implementation of hessianBlockDiagonal --- gtsam_unstable/slam/ImplicitSchurFactor.h | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam_unstable/slam/ImplicitSchurFactor.h index 0afdbcb71..5c797fccd 100644 --- a/gtsam_unstable/slam/ImplicitSchurFactor.h +++ b/gtsam_unstable/slam/ImplicitSchurFactor.h @@ -189,19 +189,24 @@ public: /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const { std::map blocks; + // F'*(I - E*P*E')*F for (size_t pos = 0; pos < size(); ++pos) { Key j = keys_[pos]; - const Matrix2D& Fj = Fblocks_[pos].second; // F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) - Eigen::Matrix FtE = Fj.transpose() - * E_.block<2, 3>(2 * pos, 0); - blocks[j] = Fj.transpose() * Fj - - FtE * PointCovariance_ * FtE.transpose(); + const Matrix2D& Fj = Fblocks_[pos].second; + // Eigen::Matrix FtE = Fj.transpose() + // * E_.block<2, 3>(2 * pos, 0); + // blocks[j] = Fj.transpose() * Fj + // - FtE * PointCovariance_ * FtE.transpose(); + + const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0); + blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); + // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( -// static const Eigen::Matrix I2 = eye(2); -// Eigen::Matrix Q = // -// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); -// blocks[j] = Fj.transpose() * Q * Fj; + // static const Eigen::Matrix I2 = eye(2); + // Eigen::Matrix Q = // + // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); + // blocks[j] = Fj.transpose() * Q * Fj; } return blocks; } From a95126599fd354bda9d941468027dc00a8007103 Mon Sep 17 00:00:00 2001 From: Zsolt Kira Date: Thu, 24 Apr 2014 09:47:48 -0400 Subject: [PATCH 16/70] Fix LM behavior when GaussianFactorGraph is subclassed. Use clone method instead of assignment to local GaussianFactorGraph when creating damped system. --- gtsam/linear/GaussianFactorGraph.cpp | 7 +++++++ gtsam/linear/GaussianFactorGraph.h | 8 +++++++- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 10 ++++++---- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 2 +- 4 files changed, 21 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index b56270a55..d6e4663e3 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -75,6 +75,13 @@ namespace gtsam { return dims_accumulated; } + /* ************************************************************************* */ + GaussianFactorGraph::shared_ptr GaussianFactorGraph::cloneToPtr() const { + gtsam::GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + *result = *this; + return result; + } + /* ************************************************************************* */ GaussianFactorGraph GaussianFactorGraph::clone() const { GaussianFactorGraph result; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index ae5730b68..692310f26 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -160,7 +160,13 @@ namespace gtsam { * Cloning preserves null factors so indices for the original graph are still * valid for the cloned graph. */ - GaussianFactorGraph clone() const; + virtual GaussianFactorGraph clone() const; + + /** + * CloneToPtr() performs a simple assignment to a new graph and returns it. + * There is no preservation of null factors! + */ + virtual GaussianFactorGraph::shared_ptr cloneToPtr() const; /** * Returns the negation of all factors in this graph - corresponds to antifactors. diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index e6c24f050..187abece6 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -138,7 +138,7 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { } /* ************************************************************************* */ -GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( +GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( const GaussianFactorGraph& linear) { gttic(damp); @@ -159,7 +159,8 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( // for each of the variables, add a prior double sigma = 1.0 / std::sqrt(state_.lambda); - GaussianFactorGraph damped = linear; + GaussianFactorGraph::shared_ptr dampedPtr = linear.cloneToPtr(); + GaussianFactorGraph &damped = (*dampedPtr); damped.reserve(damped.size() + state_.values.size()); if (params_.diagonalDamping) { BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) { @@ -188,7 +189,7 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( } } gttoc(damp); - return damped; + return dampedPtr; } /* ************************************************************************* */ @@ -212,7 +213,8 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "trying lambda = " << state_.lambda << endl; // Build damped system for this lambda (adds prior factors that make it like gradient descent) - GaussianFactorGraph dampedSystem = buildDampedSystem(*linear); + GaussianFactorGraph::shared_ptr dampedSystemPtr = buildDampedSystem(*linear); + GaussianFactorGraph &dampedSystem = (*dampedSystemPtr); // Try solving double modelFidelity = 0.0; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 47952f9e4..211830ed8 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -255,7 +255,7 @@ public: } /** Build a damped system for a specific lambda */ - GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear); + GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear); friend class ::NonlinearOptimizerMoreOptimizationTest; /// @} From 185c572b5a03acf6ac3ee83d132356bae51fbdc0 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 24 Apr 2014 11:28:41 -0400 Subject: [PATCH 17/70] removed wrong base class --- gtsam_unstable/slam/ImplicitSchurFactor.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam_unstable/slam/ImplicitSchurFactor.h index 5c797fccd..c0f339b30 100644 --- a/gtsam_unstable/slam/ImplicitSchurFactor.h +++ b/gtsam_unstable/slam/ImplicitSchurFactor.h @@ -24,7 +24,6 @@ class ImplicitSchurFactor: public GaussianFactor { public: typedef ImplicitSchurFactor This; ///< Typedef to this class - typedef JacobianFactor Base; ///< Typedef to base class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class protected: From f15f7f38932d7b170e1c54704d4d6634523eab52 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 24 Apr 2014 12:02:11 -0400 Subject: [PATCH 18/70] Jacobian factors' gradientAtZero with raw memory access does nothing (to be fixed) --- gtsam/linear/JacobianFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index bb9d5afc0..a63bbf473 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -575,7 +575,7 @@ VectorValues JacobianFactor::gradientAtZero() const { /* ************************************************************************* */ void JacobianFactor::gradientAtZero(double* d) const { - throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); + //throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); } /* ************************************************************************* */ From 4769e3c3fa10f033db2a263ee6c64a549bd44ddb Mon Sep 17 00:00:00 2001 From: Zsolt Kira Date: Thu, 24 Apr 2014 12:05:37 -0400 Subject: [PATCH 19/70] Fix for unit test based on previous LM change --- tests/testNonlinearOptimizer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index b976cca90..6d8a056fc 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -295,7 +295,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { // test the diagonal GaussianFactorGraph::shared_ptr linear = optimizer.linearize(); - GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear); + GaussianFactorGraph damped = *optimizer.buildDampedSystem(*linear); VectorValues d = linear->hessianDiagonal(), // expectedDiagonal = d + params.lambdaInitial * d; EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal())); @@ -309,7 +309,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { EXPECT(assert_equal(expectedGradient,linear->gradientAtZero())); // Check that the gradient is zero for damped system (it is not!) - damped = optimizer.buildDampedSystem(*linear); + damped = *optimizer.buildDampedSystem(*linear); VectorValues actualGradient = damped.gradientAtZero(); EXPECT(assert_equal(expectedGradient,actualGradient)); From 15201bda0f741c512218affe68a0c37cf2e31a5d Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 28 Apr 2014 13:16:25 -0400 Subject: [PATCH 20/70] fixed landmark key --- gtsam/slam/dataset.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4a7166f38..60c17ec9e 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -706,7 +706,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, Values& va } for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point - Key pointKey = symbol('l',j); + Key pointKey = P(j); if(values.exists(pointKey)){ Point3 point = values.at(pointKey); dataValues.tracks[j].p = point; From f1fb2c09a4f835700130b04f0b2d57645a720a3b Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 2 May 2014 15:01:42 -0400 Subject: [PATCH 21/70] bug fix in LM --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 187abece6..f3c145ffa 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -268,14 +268,17 @@ void LevenbergMarquardtOptimizer::iterate() { // cost change in the original, nonlinear system (old - new) double costChange = state_.error - newError; - if (linearizedCostChange > 1e-15) { // the error has to decrease to satisfy this condition + if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition // fidelity of linearized model VS original system between modelFidelity = costChange / linearizedCostChange; // if we decrease the error in the nonlinear system and modelFidelity is above threshold step_is_successful = modelFidelity > params_.minModelFidelity; - } else { - step_is_successful = true; // linearizedCostChange close to zero + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "modelFidelity: " << modelFidelity << endl; } + //else { + // step_is_successful = true; // linearizedCostChange close to zero + //} double minAbsoluteTolerance = params_.relativeErrorTol * state_.error; // if the change is small we terminate From c1650f72fdd80f6a1d1bc6c0b865db83ce2da142 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 2 May 2014 15:23:19 -0400 Subject: [PATCH 22/70] added comments --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index f3c145ffa..689642e7f 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -275,10 +275,7 @@ void LevenbergMarquardtOptimizer::iterate() { step_is_successful = modelFidelity > params_.minModelFidelity; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "modelFidelity: " << modelFidelity << endl; - } - //else { - // step_is_successful = true; // linearizedCostChange close to zero - //} + } // else we consider the step non successful and we either increase lambda or stop if error change is small double minAbsoluteTolerance = params_.relativeErrorTol * state_.error; // if the change is small we terminate From 9f499d2257756c49f45bef6f5bc685944fad4ffd Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 2 May 2014 16:05:08 -0400 Subject: [PATCH 23/70] fixed key in unit test --- gtsam/slam/tests/testDataset.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index f9a2cb34f..4ae30f508 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -19,10 +19,12 @@ #include +#include #include #include #include +using namespace gtsam::symbol_shorthand; using namespace std; using namespace gtsam; @@ -176,7 +178,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ value.insert(poseKey, pose); } for(size_t j=0; j < readData.number_tracks(); j++){ // for each point - Key pointKey = symbol('l',j); + Key pointKey = P(j); Point3 point = poseChange.transform_from( readData.tracks[j].p ); value.insert(pointKey, point); } @@ -208,7 +210,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); Point3 expectedPoint = track0.p; - Key pointKey = symbol('l',0); + Key pointKey = P(0); Point3 actualPoint = value.at(pointKey); EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); } From e8a47675914f3a8b6910afd6c9c75e459e3f6b1b Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 2 May 2014 16:10:51 -0400 Subject: [PATCH 24/70] cleaned code to log LM iterations --- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 33 ++++++++++++------- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 3 +- 2 files changed, 23 insertions(+), 13 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 689642e7f..2cde6768f 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -192,6 +192,19 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( return dampedPtr; } +/* ************************************************************************* */ +// Log current error/lambda to file +inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){ + if (!params_.logFile.empty()) { + ofstream os(params_.logFile.c_str(), ios::app); + boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time(); + os << /*inner iterations*/ state_.totalNumberInnerIterations << "," + << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," + << /*current error*/ currentError << "," << state_.lambda << "," + << /*outer iterations*/ state_.iterations << endl; + } +} + /* ************************************************************************* */ void LevenbergMarquardtOptimizer::iterate() { @@ -206,6 +219,9 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); + if(state_.totalNumberInnerIterations==0) // write initial error + writeLogFile(state_.error); + // Keep increasing lambda until we make make progress while (true) { @@ -288,36 +304,29 @@ void LevenbergMarquardtOptimizer::iterate() { } } - // Log current error/lambda to file - if (!params_.logFile.empty()) { - ofstream os(params_.logFile.c_str(), ios::app); - boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time(); - os << /*inner iterations*/ state_.totalNumberInnerIterations << "," - << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," - << /*current error*/ state_.error << "," << state_.lambda << "," - << /*outer iterations*/ state_.iterations << endl; - } ++state_.totalNumberInnerIterations; if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity state_.values.swap(newValues); state_.error = newError; decreaseLambda(modelFidelity); + writeLogFile(state_.error); break; } else if (!stopSearchingLambda) { // we failed to solved the system or we had no decrease in cost if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "increasing lambda" << endl; increaseLambda(); + writeLogFile(state_.error); // check if lambda is too big if (state_.lambda >= params_.lambdaUpperBound) { if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION) - cout - << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" - << endl; + cout << "Warning: Levenberg-Marquardt giving up because " + "cannot decrease error with maximum lambda" << endl; break; } } else { // the change in the cost is very small and it is not worth trying bigger lambdas + writeLogFile(state_.error); if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl; break; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 211830ed8..f365fc524 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -113,7 +113,6 @@ public: inline void setDiagonalDamping(bool flag) { diagonalDamping = flag; } - inline void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor_ = flag; } @@ -258,6 +257,8 @@ public: GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear); friend class ::NonlinearOptimizerMoreOptimizationTest; + void writeLogFile(double currentError); + /// @} protected: From 5c420f1aecb7eaa95056e2a3b7a5b1eb2fb4a1e8 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 4 May 2014 18:16:40 -0400 Subject: [PATCH 25/70] added references --- gtsam/navigation/CombinedImuFactor.h | 5 +++++ gtsam/navigation/ImuFactor.h | 8 +++++++- gtsam_unstable/slam/SmartProjectionPoseFactor.h | 10 ++++++++++ 3 files changed, 22 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 559568c84..06e79324d 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -34,6 +34,11 @@ namespace gtsam { * * @addtogroup SLAM * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * * REFERENCES: * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b1e20297e..f5b5293f8 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -33,7 +33,13 @@ namespace gtsam { /** * * @addtogroup SLAM - * * REFERENCES: + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + ** REFERENCES: * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam_unstable/slam/SmartProjectionPoseFactor.h index 879e5ab80..8f9617561 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactor.h @@ -22,6 +22,16 @@ #include "SmartProjectionFactor.h" namespace gtsam { +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + */ /** * The calibration is known here. The factor only constraints poses (variable dimension is 6) From 814b01a995f54911d1c35d61aa4e5b7cd2162c0d Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Thu, 22 May 2014 12:09:31 -0400 Subject: [PATCH 26/70] Added a fix to install unsupported Eigen --- gtsam/3rdparty/CMakeLists.txt | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 5822a51f5..666c340f5 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -16,13 +16,30 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() endforeach(eigen_dir) + # do the same for the unsupported eigen folder + file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h") + + file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*") + foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all}) + get_filename_component(filename ${unsupported_eigen_dir} NAME) + if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) + list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}") + install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/unsupported/Eigen) + endif() + endforeach(unsupported_eigen_dir) + # Add to project source set(eigen_headers ${eigen_headers} PARENT_SCOPE) + # set(unsupported_eigen_headers ${unsupported_eigen_headers} PARENT_SCOPE) # install Eigen - only the headers in our 3rdparty directory install(DIRECTORY Eigen/Eigen DESTINATION include/gtsam/3rdparty/Eigen FILES_MATCHING PATTERN "*.h") + + install(DIRECTORY Eigen/unsupported/Eigen + DESTINATION include/gtsam/3rdparty/unsupported/ + FILES_MATCHING PATTERN "*.h") endif() option(GTSAM_BUILD_METIS "Build metis library" ON) From b08894564146d252c4a5de019ad40783c5cdd6b3 Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Thu, 22 May 2014 13:33:02 -0400 Subject: [PATCH 27/70] In response to Chris's comment --- gtsam/3rdparty/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 666c340f5..576da93bd 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -24,7 +24,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) get_filename_component(filename ${unsupported_eigen_dir} NAME) if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}") - install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/unsupported/Eigen) + install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen) endif() endforeach(unsupported_eigen_dir) @@ -38,7 +38,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) FILES_MATCHING PATTERN "*.h") install(DIRECTORY Eigen/unsupported/Eigen - DESTINATION include/gtsam/3rdparty/unsupported/ + DESTINATION include/gtsam/3rdparty/Eigen/unsupported/ FILES_MATCHING PATTERN "*.h") endif() From 4f91f94d1e8f03d29385ab7830a7dcbb331704fe Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 22 May 2014 21:27:46 -0400 Subject: [PATCH 28/70] included possibility to linearize to Jacobian for smart Pose factors --- gtsam_unstable/slam/SmartFactorBase.h | 12 ++++++++++ gtsam_unstable/slam/SmartProjectionFactor.h | 13 +++++++++++ .../slam/SmartProjectionPoseFactor.h | 22 +++++++++++++++---- 3 files changed, 43 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index d24a878bb..265df5ecd 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -20,6 +20,7 @@ #pragma once #include "JacobianFactorQ.h" +#include "JacobianFactorSVD.h" #include "ImplicitSchurFactor.h" #include "RegularHessianFactor.h" @@ -629,6 +630,17 @@ public: return boost::make_shared < JacobianFactorQ > (Fblocks, E, PointCov, b); } + // **************************************************************************************************** + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, const Point3& point, double lambda = 0.0) const { + size_t numKeys = this->keys_.size(); + std::vector < KeyMatrix2D > Fblocks; + Vector b; + Matrix Enull(2*numKeys, 2*numKeys-3); + computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); + return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b); + } + private: /// Serialization function diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 59a841813..786c578fd 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -54,6 +54,10 @@ public: double f; }; +enum linearizationType { + HESSIAN, JACOBIAN_SVD, JACOBIAN_Q +}; + /** * SmartProjectionFactor: triangulates point * TODO: why LANDMARK parameter? @@ -400,6 +404,15 @@ public: return boost::shared_ptr >(); } + /// different (faster) way to compute Jacobian factor + boost::shared_ptr createJacobianSVDFactor(const Cameras& cameras, + double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianQFactor(cameras, point_, lambda); + else + return boost::shared_ptr >(); + } + /// Returns true if nonDegenerate bool computeCamerasAndTriangulate(const Values& values, Cameras& myCameras) const { diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam_unstable/slam/SmartProjectionPoseFactor.h index 879e5ab80..4a565d7dd 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactor.h @@ -31,6 +31,8 @@ template class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: + linearizationType linearizeTo_; + // Known calibration std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) @@ -56,8 +58,9 @@ public: */ SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {} + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + linearizationType linearizeTo = HESSIAN) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor), linearizeTo_(linearizeTo) {} /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} @@ -139,11 +142,22 @@ public: } /** - * linearize returns a Hessian factor contraining the poses + * linear factor on the poses */ virtual boost::shared_ptr linearize( const Values& values) const { - return this->createHessianFactor(cameras(values)); + // depending on flag set on construction we may linearize to different linear factors + switch(linearizeTo_){ + case JACOBIAN_SVD : + return this->createJacobianSVDFactor(cameras(values), 0.0); + break; + case JACOBIAN_Q : + return this->createJacobianQFactor(cameras(values), 0.0); + break; + default: + return this->createHessianFactor(cameras(values)); + break; + } } /** From e0500f7b186c8c770e4d77c6504c86022c7607e2 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 22 May 2014 21:28:00 -0400 Subject: [PATCH 29/70] added and renamed unit tests --- .cproject | 378 +++++++++--------- ....cpp => testSmartProjectionPoseFactor.cpp} | 124 ++++++ 2 files changed, 317 insertions(+), 185 deletions(-) rename gtsam_unstable/slam/tests/{testSmartProjectionHessianPoseFactor.cpp => testSmartProjectionPoseFactor.cpp} (89%) diff --git a/.cproject b/.cproject index 7a99a565d..8e20710ec 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -62,13 +60,13 @@ - - + + @@ -118,13 +116,13 @@ - - + + @@ -542,14 +540,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -576,6 +566,7 @@ make + tests/testBayesTree.run true false @@ -583,6 +574,7 @@ make + testBinaryBayesNet.run true false @@ -630,6 +622,7 @@ make + testSymbolicBayesNet.run true false @@ -637,6 +630,7 @@ make + tests/testSymbolicFactor.run true false @@ -644,6 +638,7 @@ make + testSymbolicFactorGraph.run true false @@ -659,11 +654,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -888,22 +892,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -920,6 +908,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -1120,6 +1124,14 @@ true true + + make + -j5 + testSmartProjectionPoseFactor.run + true + true + true + make -j2 @@ -1304,14 +1316,6 @@ true true - - make - -j5 - testSmartProjectionFactor.run - true - true - true - make -j5 @@ -1530,6 +1534,7 @@ make + testGraph.run true false @@ -1537,6 +1542,7 @@ make + testJunctionTree.run true false @@ -1544,6 +1550,7 @@ make + testSymbolicBayesNetB.run true false @@ -1711,6 +1718,7 @@ make + testErrors.run true false @@ -1756,22 +1764,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1852,6 +1844,22 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -2004,22 +2012,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j2 @@ -2102,7 +2094,6 @@ make - testSimulated2DOriented.run true false @@ -2142,7 +2133,6 @@ make - testSimulated2D.run true false @@ -2150,7 +2140,6 @@ make - testSimulated3D.run true false @@ -2164,6 +2153,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j5 @@ -2390,7 +2395,6 @@ make - tests/testGaussianISAM2 true false @@ -2412,102 +2416,6 @@ 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 @@ -2709,6 +2617,7 @@ cpack + -G DEB true false @@ -2716,6 +2625,7 @@ cpack + -G RPM true false @@ -2723,6 +2633,7 @@ cpack + -G TGZ true false @@ -2730,6 +2641,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2903,34 +2815,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -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 @@ -2974,6 +2950,38 @@ 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/tests/testSmartProjectionHessianPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp similarity index 89% rename from gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp rename to gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp index 9a3fe7f62..e55084ed7 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp @@ -369,6 +369,130 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ if(isDebugTest) tictoc_print_(); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianSVD ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // 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); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianQ ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // 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); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; From c2705902cb05a01795098d13b48a3eef8e663810 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 22 May 2014 22:32:12 -0400 Subject: [PATCH 30/70] added dynamic outlier rejection (with unit tests) for smart pose factors --- gtsam_unstable/slam/SmartProjectionFactor.h | 41 ++++- .../slam/SmartProjectionPoseFactor.h | 6 +- .../tests/testSmartProjectionPoseFactor.cpp | 141 ++++++++++++++++++ 3 files changed, 183 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 786c578fd..33730db8a 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -95,6 +95,13 @@ protected: /// shorthand for base class type typedef SmartFactorBase Base; + double landmarkDistanceThreshold_; // if the landmark is triangulated at a + // distance larger than that the factor is considered degenerate + + double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the + // average reprojection error is smaller than this threshold after triangulation, + // and the factor is disregarded if the error is large + /// shorthand for this class typedef SmartProjectionFactor This; @@ -119,12 +126,15 @@ public: SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, boost::optional body_P_sensor = boost::none, - SmartFactorStatePtr state = SmartFactorStatePtr( - new SmartProjectionFactorState())) : + double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1, + SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state) { + false), verboseCheirality_(false), state_(state), + landmarkDistanceThreshold_(landmarkDistanceThreshold), + dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { } /** Virtual destructor */ @@ -238,6 +248,31 @@ public: rankTolerance_, enableEPI_); degenerate_ = false; cheiralityException_ = false; + + // Check landmark distance and reprojection errors to avoid outliers + double totalReprojError = 0.0; + size_t i=0; + BOOST_FOREACH(const Camera& camera, cameras) { + Point3 cameraTranslation = camera.pose().translation(); + // we discard smart factors corresponding to points that are far away + if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + degenerate_ = true; + break; + } + const Point2& zi = this->measured_.at(i); + try { + Point2 reprojectionError(camera.project(point_) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException& e) { + cheiralityException_ = true; + } + i += 1; + } + // we discard smart factors that have large reprojection error + if(dynamicOutlierRejectionThreshold_ > 0 && + totalReprojError/m > dynamicOutlierRejectionThreshold_) + degenerate_ = true; + } catch (TriangulationUnderconstrainedException&) { // if TriangulationUnderconstrainedException can be // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam_unstable/slam/SmartProjectionPoseFactor.h index 4a565d7dd..9548e10c8 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactor.h @@ -59,8 +59,10 @@ public: SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, const bool enableEPI = false, boost::optional body_P_sensor = boost::none, - linearizationType linearizeTo = HESSIAN) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor), linearizeTo_(linearizeTo) {} + linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp index e55084ed7..ee8db1267 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp @@ -431,6 +431,147 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ EXPECT(assert_equal(pose3,result.at(x3))); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, landmarkDistance ){ + + double excludeLandmarksFutherThanDist = 2; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // 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); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3),result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // 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); + Point3 landmark4(5, -0.5, 1); + + vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, + JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor3->add(measurements_cam3, views, model, K); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor4->add(measurements_cam4, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, jacobianQ ){ From 8806c15b36d94d3bfd96fe5b91e5c9f41a192022 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 23 May 2014 17:49:42 -0400 Subject: [PATCH 31/70] fixed possible connectivity issue when smart factors are degenerate --- .cproject | 354 ++++++++++---------- gtsam_unstable/slam/JacobianFactorQ.h | 13 + gtsam_unstable/slam/JacobianFactorSVD.h | 14 +- gtsam_unstable/slam/SmartProjectionFactor.h | 10 +- 4 files changed, 207 insertions(+), 184 deletions(-) diff --git a/.cproject b/.cproject index 8e20710ec..fa8c24875 100644 --- a/.cproject +++ b/.cproject @@ -540,6 +540,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -566,7 +574,6 @@ make - tests/testBayesTree.run true false @@ -574,7 +581,6 @@ make - testBinaryBayesNet.run true false @@ -622,7 +628,6 @@ make - testSymbolicBayesNet.run true false @@ -630,7 +635,6 @@ make - tests/testSymbolicFactor.run true false @@ -638,7 +642,6 @@ make - testSymbolicFactorGraph.run true false @@ -654,20 +657,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -892,22 +886,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -924,6 +902,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -1260,6 +1254,14 @@ true true + + make + -j5 + testAttitudeFactor.run + true + true + true + make -j5 @@ -1534,7 +1536,6 @@ make - testGraph.run true false @@ -1542,7 +1543,6 @@ make - testJunctionTree.run true false @@ -1550,7 +1550,6 @@ make - testSymbolicBayesNetB.run true false @@ -1718,7 +1717,6 @@ make - testErrors.run true false @@ -1764,6 +1762,22 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1844,22 +1858,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -2012,6 +2010,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j2 @@ -2094,6 +2108,7 @@ make + testSimulated2DOriented.run true false @@ -2133,6 +2148,7 @@ make + testSimulated2D.run true false @@ -2140,6 +2156,7 @@ make + testSimulated3D.run true false @@ -2153,22 +2170,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j5 @@ -2395,6 +2396,7 @@ make + tests/testGaussianISAM2 true false @@ -2416,6 +2418,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 @@ -2617,7 +2715,6 @@ cpack - -G DEB true false @@ -2625,7 +2722,6 @@ cpack - -G RPM true false @@ -2633,7 +2729,6 @@ cpack - -G TGZ true false @@ -2641,7 +2736,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2815,98 +2909,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 @@ -2950,38 +2980,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/JacobianFactorQ.h b/gtsam_unstable/slam/JacobianFactorQ.h index fdbe0e231..f4dfb9422 100644 --- a/gtsam_unstable/slam/JacobianFactorQ.h +++ b/gtsam_unstable/slam/JacobianFactorQ.h @@ -23,6 +23,19 @@ public: JacobianFactorQ() { } + /// Empty constructor with keys + JacobianFactorQ(const FastVector& keys, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + Matrix zeroMatrix = Matrix::Zero(0,D); + Vector zeroVector = Vector::Zero(0); + typedef std::pair KeyMatrix; + std::vector QF; + QF.reserve(keys.size()); + BOOST_FOREACH(const Key& key, keys) + QF.push_back(KeyMatrix(key, zeroMatrix)); + JacobianFactor::fillTerms(QF, zeroVector, model); + } + /// Constructor JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, diff --git a/gtsam_unstable/slam/JacobianFactorSVD.h b/gtsam_unstable/slam/JacobianFactorSVD.h index 752a9f48e..e8ade3b1b 100644 --- a/gtsam_unstable/slam/JacobianFactorSVD.h +++ b/gtsam_unstable/slam/JacobianFactorSVD.h @@ -18,10 +18,23 @@ public: typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; + typedef std::pair KeyMatrix; /// Default constructor JacobianFactorSVD() {} + /// Empty constructor with keys + JacobianFactorSVD(const FastVector& keys, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + Matrix zeroMatrix = Matrix::Zero(0,D); + Vector zeroVector = Vector::Zero(0); + std::vector QF; + QF.reserve(keys.size()); + BOOST_FOREACH(const Key& key, keys) + QF.push_back(KeyMatrix(key, zeroMatrix)); + JacobianFactor::fillTerms(QF, zeroVector, model); + } + /// Constructor JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { @@ -32,7 +45,6 @@ public: // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); - typedef std::pair KeyMatrix; std::vector QF; QF.reserve(numKeys); BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 33730db8a..2124dd6f6 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -424,7 +424,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// Create a factor, takes values @@ -436,16 +436,16 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// different (faster) way to compute Jacobian factor - boost::shared_ptr createJacobianSVDFactor(const Cameras& cameras, + boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createJacobianQFactor(cameras, point_, lambda); + return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate From 87c386d77fd67f6b6b4f18173513ae587167162a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 02:03:33 -0400 Subject: [PATCH 32/70] Fast creation of large key sets from within MATLAB --- gtsam.h | 1 + matlab.h | 8 ++++++++ matlab/+gtsam/Contents.m | 1 + 3 files changed, 10 insertions(+) diff --git a/gtsam.h b/gtsam.h index 2d181d350..71fcaeb17 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2375,6 +2375,7 @@ namespace utilities { void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + gtsam::KeySet createKeySet(string s, Vector I); } //\namespace utilities diff --git a/matlab.h b/matlab.h index 100f5a242..261fd2d48 100644 --- a/matlab.h +++ b/matlab.h @@ -151,6 +151,14 @@ namespace gtsam { return errors; } + // Create a Keyset from indices + FastSet createKeySet(string s, const Vector& I) { + FastSet set; + char c = s[0]; + for(int i=0;i Date: Sun, 25 May 2014 10:49:59 -0400 Subject: [PATCH 33/70] Formatting --- matlab.h | 272 +++++++++++++++++++++++++++++-------------------------- 1 file changed, 144 insertions(+), 128 deletions(-) diff --git a/matlab.h b/matlab.h index 261fd2d48..b62c9866a 100644 --- a/matlab.h +++ b/matlab.h @@ -32,134 +32,150 @@ namespace gtsam { - namespace utilities { - - /// Extract all Point2 values into a single matrix [x y] - Matrix extractPoint2(const Values& values) { - size_t j=0; - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),2); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Point3 values into a single matrix [x y z] - Matrix extractPoint3(const Values& values) { - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Pose2 values into a single matrix [x y theta] - Matrix extractPose2(const Values& values) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) - result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); - return result; - } - - /// Extract all Pose3 values - Values allPose3s(const Values& values) { - return values.filter(); - } - - /// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] - Matrix extractPose3(const Values& values) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),12); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { - result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); - result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); - result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); - result.row(j).tail(3) = key_value.value.translation().vector(); - j++; - } - return result; - } - - /// Perturb all Point2 values using normally distributed noise - void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Perturb all Pose2 values using normally distributed noise - void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = 42u) { - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( - Vector3(sigmaT, sigmaT, sigmaR)); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Perturb all Point3 values using normally distributed noise - void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Insert a number of initial point values by backprojecting - void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { - if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); - for(int k=0;k > - (Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); - } - } - - /// Calculate the errors of all projection factors in a graph - Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) - if (boost::dynamic_pointer_cast >(f)) ++K; - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { - boost::shared_ptr > p = boost::dynamic_pointer_cast >(f); - if (p) errors.col(k++) = p->unwhitenedError(values); - } - return errors; - } - - // Create a Keyset from indices - FastSet createKeySet(string s, const Vector& I) { - FastSet set; - char c = s[0]; - for(int i=0;i points = values.filter(); + Matrix result(points.size(), 2); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Point3 values into a single matrix [x y z] +Matrix extractPoint3(const Values& values) { + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Pose2 values into a single matrix [x y theta] +Matrix extractPose2(const Values& values) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) + result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); + return result; +} + +/// Extract all Pose3 values +Values allPose3s(const Values& values) { + return values.filter(); +} + +/// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] +Matrix extractPose3(const Values& values) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 12); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { + result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); + result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); + result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); + result.row(j).tail(3) = key_value.value.translation().vector(); + j++; + } + return result; +} + +/// Perturb all Point2 values using normally distributed noise +void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Pose2 values using normally distributed noise +void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = + 42u) { + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( + Vector3(sigmaT, sigmaT, sigmaR)); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Point3 values using normally distributed noise +void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Insert a number of initial point values by backprojecting +void insertBackprojections(Values& values, const SimpleCamera& camera, + const Vector& J, const Matrix& Z, double depth) { + if (Z.rows() != 2) + throw std::invalid_argument("insertBackProjections: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "insertBackProjections: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + Point2 p(Z(0, k), Z(1, k)); + Point3 P = camera.backproject(p, depth); + values.insert(J(k), P); + } +} + +/// Insert multiple projection factors for a single pose key +void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, + const Vector& J, const Matrix& Z, const SharedNoiseModel& model, + const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { + if (Z.rows() != 2) + throw std::invalid_argument("addMeasurements: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "addMeasurements: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + graph.push_back( + boost::make_shared >( + Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); + } +} + +/// Calculate the errors of all projection factors in a graph +Matrix reprojectionErrors(const NonlinearFactorGraph& graph, + const Values& values) { + // first count + size_t K = 0, k = 0; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) + if (boost::dynamic_pointer_cast >( + f)) + ++K; + // now fill + Matrix errors(2, K); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { + boost::shared_ptr > p = + boost::dynamic_pointer_cast >( + f); + if (p) + errors.col(k++) = p->unwhitenedError(values); + } + return errors; +} + +// Create a Keyset from indices +FastSet createKeySet(string s, const Vector& I) { + FastSet set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.insert(symbol(c, I[i])); + return set; +} + +} } From ddcf9c0efb622515a7079e36614500d8e33096a7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 11:15:17 -0400 Subject: [PATCH 34/70] New routines to create Key collections --- gtsam.h | 7 ++++++- matlab.h | 60 +++++++++++++++++++++++++++++++++++++++++++++++--------- 2 files changed, 57 insertions(+), 10 deletions(-) diff --git a/gtsam.h b/gtsam.h index 71fcaeb17..99aec3f86 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2363,6 +2363,12 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { namespace utilities { #include + gtsam::KeyList createKeyList(Vector I); + gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + gtsam::KeySet createKeySet(Vector I); + gtsam::KeySet createKeySet(string s, Vector I); Matrix extractPoint2(const gtsam::Values& values); Matrix extractPoint3(const gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values); @@ -2375,7 +2381,6 @@ namespace utilities { void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); - gtsam::KeySet createKeySet(string s, Vector I); } //\namespace utilities diff --git a/matlab.h b/matlab.h index b62c9866a..e9ad519bd 100644 --- a/matlab.h +++ b/matlab.h @@ -34,6 +34,57 @@ namespace gtsam { namespace utilities { +// Create a KeyList from indices +FastList createKeyList(const Vector& I) { + FastList set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyList from indices using symbol +FastList createKeyList(string s, const Vector& I) { + FastList set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeyVector from indices +FastVector createKeyVector(const Vector& I) { + FastVector set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyVector from indices using symbol +FastVector createKeyVector(string s, const Vector& I) { + FastVector set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeySet from indices +FastSet createKeySet(const Vector& I) { + FastSet set; + for (int i = 0; i < I.size(); i++) + set.insert(I[i]); + return set; +} + +// Create a KeySet from indices using symbol +FastSet createKeySet(string s, const Vector& I) { + FastSet set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.insert(symbol(c, I[i])); + return set; +} + /// Extract all Point2 values into a single matrix [x y] Matrix extractPoint2(const Values& values) { size_t j = 0; @@ -167,15 +218,6 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph, return errors; } -// Create a Keyset from indices -FastSet createKeySet(string s, const Vector& I) { - FastSet set; - char c = s[0]; - for (int i = 0; i < I.size(); i++) - set.insert(symbol(c, I[i])); - return set; -} - } } From ab4bb159e8871b4e4dc51d0774c4c421026f1973 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 11:15:49 -0400 Subject: [PATCH 35/70] Unit tests for new routines (and they *failed* at first, as I had a bug) --- matlab/gtsam_tests/testUtilities.m | 47 ++++++++++++++++++++++++++++++ matlab/gtsam_tests/test_gtsam.m | 3 ++ 2 files changed, 50 insertions(+) create mode 100644 matlab/gtsam_tests/testUtilities.m diff --git a/matlab/gtsam_tests/testUtilities.m b/matlab/gtsam_tests/testUtilities.m new file mode 100644 index 000000000..da8dec789 --- /dev/null +++ b/matlab/gtsam_tests/testUtilities.m @@ -0,0 +1,47 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief Checks for results of functions in utilities namespace +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +import gtsam.* + +%% Create keys for variables +x1 = symbol('x',1); x2 = symbol('x',2); x3 = symbol('x',3); + +actual = utilities.createKeyList([1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==1', actual.front==1); + +actual = utilities.createKeyList('x',[1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==x1', actual.front==x1); + +actual = utilities.createKeyVector([1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==1', actual.at(0)==1); + +actual = utilities.createKeyVector('x',[1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==x1', actual.at(0)==x1); + +actual = utilities.createKeySet([1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(1)', actual.count(1)); + +actual = utilities.createKeySet('x',[1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(x1)', actual.count(x1)); + diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 2cad40df8..c379179c5 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -33,5 +33,8 @@ testVisualISAMExample display 'Starting: testSerialization' testSerialization +display 'Starting: testUtilities' +testUtilities + % end of tests display 'Tests complete!' From 1762825c289c3d579fd5a0201d90f31419385cc1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 11:17:49 -0400 Subject: [PATCH 36/70] Forgot to update docs --- matlab/+gtsam/Contents.m | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index ac6842e8e..7a7491462 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -175,6 +175,9 @@ % symbolIndex - get index from a symbol key % %% Wrapped C++ Convenience Functions for use within MATLAB +% utilities.createKeyList - Create KeyList from indices +% utilities.createKeyVector - Create KeyVector from indices +% utilities.createKeySet - Create KeySet from indices % utilities.extractPoint2 - Extract all Point2 values into a single matrix [x y] % utilities.extractPoint3 - Extract all Point3 values into a single matrix [x y z] % utilities.extractPose2 - Extract all Pose2 values into a single matrix [x y theta] @@ -186,4 +189,3 @@ % utilities.insertBackprojections - Insert a number of initial point values by backprojecting % utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key % utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph -% utilities.createKeySet - Create keys from indices From 02c3fe9516e128dd50ec3ea55a2bfec6de846861 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 12:06:34 -0400 Subject: [PATCH 37/70] Standard BORG formatting --- wrap/Method.cpp | 168 +++++++++++++++++++++++++----------------------- 1 file changed, 88 insertions(+), 80 deletions(-) diff --git a/wrap/Method.cpp b/wrap/Method.cpp index b327ac7dc..3fbf6a9c0 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -38,57 +38,54 @@ void Method::addOverload(bool verbose, bool is_const, const std::string& name, } /* ************************************************************************* */ -void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const string& cppClassName, - const std::string& matlabQualName, - const std::string& matlabUniqueName, - const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { +void Method::proxy_wrapper_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const string& cppClassName, + const std::string& matlabQualName, const std::string& matlabUniqueName, + const string& wrapperName, const TypeAttributesTable& typeAttributes, + vector& functionNames) const { proxyFile.oss << " function varargout = " << name << "(this, varargin)\n"; //Comments for documentation - string up_name = boost::to_upper_copy(name); + string up_name = boost::to_upper_copy(name); proxyFile.oss << " % " << up_name << " usage:"; unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - if(argLCount != argLists.size()-1) - proxyFile.oss << "), "; - else - proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; + BOOST_FOREACH(ArgumentList argList, argLists) { + proxyFile.oss << " " << name << "("; + unsigned int i = 0; + BOOST_FOREACH(const Argument& arg, argList) { + if (i != argList.size() - 1) + proxyFile.oss << arg.type << " " << arg.name << ", "; + else + proxyFile.oss << arg.type << " " << arg.name; + i++; } + if (argLCount != argLists.size() - 1) + proxyFile.oss << "), "; + else + proxyFile.oss << ") : returns " + << returnVals[0].return_type(false, returnVals[0].pair) << endl; + argLCount++; + } - proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; + proxyFile.oss << " % " + << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" + << endl; proxyFile.oss << " % " << "" << endl; proxyFile.oss << " % " << "Method Overloads" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " % " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + proxyFile.oss << " % " << name << "("; + unsigned int i = 0; + BOOST_FOREACH(const Argument& arg, argList) { + if (i != argList.size() - 1) + proxyFile.oss << arg.type << " " << arg.name << ", "; + else + proxyFile.oss << arg.type << " " << arg.name; + i++; } + proxyFile.oss << ")" << endl; + } - for(size_t overload = 0; overload < argLists.size(); ++overload) { + for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; size_t nrArgs = args.size(); @@ -98,95 +95,106 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperF // Output proxy matlab code // check for number of arguments... - proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) proxyFile.oss << " && "; + proxyFile.oss << " " << (overload == 0 ? "" : "else") + << "if length(varargin) == " << nrArgs; + if (nrArgs > 0) + proxyFile.oss << " && "; // ...and their types bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + + boost::lexical_cast(id); const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; // call - file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start file.oss << "{\n"; - if(returnVal.isPair) - { - if(returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; - if(returnVal.category2 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 << ";"<< endl; - } - else - if(returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; + if (returnVal.isPair) { + if (returnVal.category1 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" + << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 + << ";" << endl; + if (returnVal.category2 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" + << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 + << ";" << endl; + } else if (returnVal.category1 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") + << "> Shared" << returnVal.type1 << ";" << endl; - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" + << endl; // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," + << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; + file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file,1); + args.matlab_unwrap(file, 1); // call method and wrap result // example: out[0]=wrap(self->return_field(t)); - if (returnVal.type1!="void") - returnVal.wrap_result("obj->"+name+"("+args.names()+")", file, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result("obj->" + name + "(" + args.names() + ")", file, + typeAttributes); else - file.oss << " obj->"+name+"("+args.names()+");\n"; + file.oss << " obj->" + name + "(" + args.names() + ");\n"; // finish file.oss << "}\n"; From 82d6bae4b9150967035825ee50d3a84db4c95ae8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 12:43:19 -0400 Subject: [PATCH 38/70] Standard BORG formatting --- wrap/Class.cpp | 719 +++++++++++++++++++++++------------------- wrap/StaticMethod.cpp | 150 ++++----- 2 files changed, 474 insertions(+), 395 deletions(-) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 3b8c41041..8788e740f 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -7,62 +7,62 @@ * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - + * -------------------------------------------------------------------------- */ + /** * @file Class.cpp * @author Frank Dellaert * @author Andrew Melim * @author Richard Roberts - **/ - + **/ + #include #include #include //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC - #include #include - + #include "Class.h" #include "utilities.h" #include "Argument.h" - -using namespace std; -using namespace wrap; - -/* ************************************************************************* */ -void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, - const TypeAttributesTable& typeAttributes, - FileWriter& wrapperFile, vector& functionNames) const { - + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, + const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, + vector& functionNames) const { + // Create namespace folders - createNamespaceStructure(namespaces, toolboxPath); - + createNamespaceStructure(namespaces, toolboxPath); + // open destination classFile - string classFile = toolboxPath; - if(!namespaces.empty()) - classFile += "/+" + wrap::qualifiedName("/+", namespaces); - classFile += "/" + name + ".m"; - FileWriter proxyFile(classFile, verbose_, "%"); - + string classFile = toolboxPath; + if (!namespaces.empty()) + classFile += "/+" + wrap::qualifiedName("/+", namespaces); + classFile += "/" + name + ".m"; + FileWriter proxyFile(classFile, verbose_, "%"); + // get the name of actual matlab object - const string matlabQualName = qualifiedName("."), matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); - const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); - const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); - + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppName = qualifiedName("::"); + const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); + const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); + // emit class proxy code // we want our class to inherit the handle class for memory purposes - const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; + const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; comment_fragment(proxyFile); - proxyFile.oss << "classdef " << name << " < " << parent << endl; + proxyFile.oss << "classdef " << name << " < " << parent << endl; proxyFile.oss << " properties\n"; proxyFile.oss << " ptr_" << matlabUniqueName << " = 0\n"; proxyFile.oss << " end\n"; proxyFile.oss << " methods\n"; - + // Constructor proxyFile.oss << " function obj = " << name << "(varargin)\n"; // Special pointer constructors - one in MATLAB to create an object and @@ -72,267 +72,316 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, // other wrap modules - to add these to their collectors the pointer is // passed from one C++ module into matlab then back into the other C++ // module. - pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, functionNames); - wrapperFile.oss << "\n"; + pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, + functionNames); + wrapperFile.oss << "\n"; // Regular constructors - BOOST_FOREACH(ArgumentList a, constructor.args_list) - { - const int id = (int)functionNames.size(); - constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), id, a); - const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, - cppName, matlabUniqueName, cppBaseName, id, a); - wrapperFile.oss << "\n"; - functionNames.push_back(wrapFunctionName); - } - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of " << matlabQualName << " constructor');\n"; - proxyFile.oss << " end\n"; - if(!qualifiedParent.empty()) - proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" << ptr_constructor_key << "), base_ptr);\n"; - proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; - proxyFile.oss << " end\n\n"; - + BOOST_FOREACH(ArgumentList a, constructor.args_list) { + const int id = (int) functionNames.size(); + constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), + id, a); + const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, + cppName, matlabUniqueName, cppBaseName, id, a); + wrapperFile.oss << "\n"; + functionNames.push_back(wrapFunctionName); + } + proxyFile.oss << " else\n"; + proxyFile.oss << " error('Arguments do not match any overload of " + << matlabQualName << " constructor');\n"; + proxyFile.oss << " end\n"; + if (!qualifiedParent.empty()) + proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" + << ptr_constructor_key << "), base_ptr);\n"; + proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; + proxyFile.oss << " end\n\n"; + // Deconstructor - { - const int id = (int)functionNames.size(); - deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); - proxyFile.oss << "\n"; - const string functionName = deconstructor.wrapper_fragment(wrapperFile, cppName, matlabUniqueName, id); - wrapperFile.oss << "\n"; - functionNames.push_back(functionName); - } - proxyFile.oss << " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n"; - proxyFile.oss << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; - + { + const int id = (int) functionNames.size(); + deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); + proxyFile.oss << "\n"; + const string functionName = deconstructor.wrapper_fragment(wrapperFile, + cppName, matlabUniqueName, id); + wrapperFile.oss << "\n"; + functionNames.push_back(functionName); + } + proxyFile.oss + << " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n"; + proxyFile.oss + << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; + // Methods - BOOST_FOREACH(const Methods::value_type& name_m, methods) { - const Method& m = name_m.second; - m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); - proxyFile.oss << "\n"; - wrapperFile.oss << "\n"; - } + BOOST_FOREACH(const Methods::value_type& name_m, methods) { + const Method& m = name_m.second; + m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, + matlabUniqueName, wrapperName, typeAttributes, functionNames); + proxyFile.oss << "\n"; + wrapperFile.oss << "\n"; + } if (hasSerialization) serialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); - - proxyFile.oss << " end\n"; - proxyFile.oss << "\n"; - proxyFile.oss << " methods(Static = true)\n"; - + + proxyFile.oss << " end\n"; + proxyFile.oss << "\n"; + proxyFile.oss << " methods(Static = true)\n"; + // Static methods - BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { - const StaticMethod& m = name_m.second; - m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); - proxyFile.oss << "\n"; - wrapperFile.oss << "\n"; - } + BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { + const StaticMethod& m = name_m.second; + m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, + matlabUniqueName, wrapperName, typeAttributes, functionNames); + proxyFile.oss << "\n"; + wrapperFile.oss << "\n"; + } if (hasSerialization) - deserialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); + deserialization_fragments(proxyFile, wrapperFile, wrapperName, + functionNames); proxyFile.oss << " end\n"; proxyFile.oss << "end\n"; - + // Close file - proxyFile.emit(true); -} - -/* ************************************************************************* */ -string Class::qualifiedName(const string& delim) const { - return ::wrap::qualifiedName(delim, namespaces, name); -} - -/* ************************************************************************* */ -void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const string& wrapperName, vector& functionNames) const { - - const string matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); - const string baseCppName = wrap::qualifiedName("::", qualifiedParent); - - const int collectorInsertId = (int)functionNames.size(); - const string collectorInsertFunctionName = matlabUniqueName + "_collectorInsertAndMakeBase_" + boost::lexical_cast(collectorInsertId); - functionNames.push_back(collectorInsertFunctionName); - - int upcastFromVoidId; - string upcastFromVoidFunctionName; - if(isVirtual) { - upcastFromVoidId = (int)functionNames.size(); - upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + boost::lexical_cast(upcastFromVoidId); - functionNames.push_back(upcastFromVoidFunctionName); - } - + proxyFile.emit(true); +} + +/* ************************************************************************* */ +string Class::qualifiedName(const string& delim) const { + return ::wrap::qualifiedName(delim, namespaces, name); +} + +/* ************************************************************************* */ +void Class::pointer_constructor_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const string& wrapperName, + vector& functionNames) const { + + const string matlabUniqueName = qualifiedName(), cppName = qualifiedName( + "::"); + const string baseCppName = wrap::qualifiedName("::", qualifiedParent); + + const int collectorInsertId = (int) functionNames.size(); + const string collectorInsertFunctionName = matlabUniqueName + + "_collectorInsertAndMakeBase_" + + boost::lexical_cast(collectorInsertId); + functionNames.push_back(collectorInsertFunctionName); + + int upcastFromVoidId; + string upcastFromVoidFunctionName; + if (isVirtual) { + upcastFromVoidId = (int) functionNames.size(); + upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + + boost::lexical_cast(upcastFromVoidId); + functionNames.push_back(upcastFromVoidFunctionName); + } + // MATLAB constructor that assigns pointer to matlab object then calls c++ // function to add the object to the collector. - if(isVirtual) { - proxyFile.oss << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; - } else { - proxyFile.oss << " if nargin == 2"; - } - proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" << ptr_constructor_key << ")\n"; - if(isVirtual) { - proxyFile.oss << " if nargin == 2\n"; - proxyFile.oss << " my_ptr = varargin{2};\n"; - proxyFile.oss << " else\n"; - proxyFile.oss << " my_ptr = " << wrapperName << "(" << upcastFromVoidId << ", varargin{2});\n"; - proxyFile.oss << " end\n"; - } else { - proxyFile.oss << " my_ptr = varargin{2};\n"; - } - if(qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back - proxyFile.oss << " "; - else - proxyFile.oss << " base_ptr = "; + if (isVirtual) { + proxyFile.oss + << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; + } else { + proxyFile.oss << " if nargin == 2"; + } + proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" + << ptr_constructor_key << ")\n"; + if (isVirtual) { + proxyFile.oss << " if nargin == 2\n"; + proxyFile.oss << " my_ptr = varargin{2};\n"; + proxyFile.oss << " else\n"; + proxyFile.oss << " my_ptr = " << wrapperName << "(" + << upcastFromVoidId << ", varargin{2});\n"; + proxyFile.oss << " end\n"; + } else { + proxyFile.oss << " my_ptr = varargin{2};\n"; + } + if (qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back + proxyFile.oss << " "; + else + proxyFile.oss << " base_ptr = "; proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr - + // C++ function to add pointer from MATLAB to collector. The pointer always // comes from a C++ return value; this mechanism allows the object to be added // to a collector in a different wrap module. If this class has a base class, // a new pointer to the base class is allocated and returned. - wrapperFile.oss << "void " << collectorInsertFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - wrapperFile.oss << "{\n"; - wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; + wrapperFile.oss << "void " << collectorInsertFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "{\n"; + wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; // Typedef boost::shared_ptr - wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; - wrapperFile.oss << "\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; + wrapperFile.oss << "\n"; // Get self pointer passed in - wrapperFile.oss << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; + wrapperFile.oss + << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; // Add to collector - wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; + wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if(!qualifiedParent.empty()) { - wrapperFile.oss << "\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName << "> SharedBase;\n"; - wrapperFile.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; - wrapperFile.oss << " *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self);\n"; - } - wrapperFile.oss << "}\n"; - + if (!qualifiedParent.empty()) { + wrapperFile.oss << "\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName + << "> SharedBase;\n"; + wrapperFile.oss + << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; + wrapperFile.oss + << " *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self);\n"; + } + wrapperFile.oss << "}\n"; + // If this is a virtual function, C++ function to dynamic upcast it from a // shared_ptr. This mechanism allows automatic dynamic creation of the // real underlying derived-most class when a C++ method returns a virtual // base class. - if(isVirtual) - wrapperFile.oss << - "\n" - "void " << upcastFromVoidFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n" - " mexAtExit(&_deleteAllObjects);\n" - " typedef boost::shared_ptr<" << cppName << "> Shared;\n" - " boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0]));\n" - " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n" - " Shared *self = new Shared(boost::static_pointer_cast<" << cppName << ">(*asVoid));\n" - " *reinterpret_cast(mxGetData(out[0])) = self;\n" - "}\n"; -} - -/* ************************************************************************* */ -vector expandArgumentListsTemplate(const vector& argLists, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - vector result; - BOOST_FOREACH(const ArgumentList& argList, argLists) { - ArgumentList instArgList; - BOOST_FOREACH(const Argument& arg, argList) { - Argument instArg = arg; - if(arg.type == templateArg) { - instArg.namespaces.assign(instName.begin(), instName.end()-1); - instArg.type = instName.back(); - } else if(arg.type == "This") { - instArg.namespaces.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instArg.type = expandedClassName; - } - instArgList.push_back(instArg); - } - result.push_back(instArgList); - } - return result; -} - -/* ************************************************************************* */ -template -map expandMethodTemplate(const map& methods, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - map result; - typedef pair Name_Method; - BOOST_FOREACH(const Name_Method& name_method, methods) { - const METHOD& method = name_method.second; - METHOD instMethod = method; - instMethod.argLists = expandArgumentListsTemplate(method.argLists, templateArg, instName, expandedClassNamespace, expandedClassName); - instMethod.returnVals.clear(); - BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { - ReturnValue instRetVal = retVal; - if(retVal.type1 == templateArg) { - instRetVal.namespaces1.assign(instName.begin(), instName.end()-1); - instRetVal.type1 = instName.back(); - } else if(retVal.type1 == "This") { - instRetVal.namespaces1.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instRetVal.type1 = expandedClassName; - } - if(retVal.type2 == templateArg) { - instRetVal.namespaces2.assign(instName.begin(), instName.end()-1); - instRetVal.type2 = instName.back(); - } else if(retVal.type1 == "This") { - instRetVal.namespaces2.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instRetVal.type2 = expandedClassName; - } - instMethod.returnVals.push_back(instRetVal); - } - result.insert(make_pair(name_method.first, instMethod)); - } - return result; -} - -/* ************************************************************************* */ -Class expandClassTemplate(const Class& cls, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - Class inst; - inst.name = cls.name; - inst.templateArgs = cls.templateArgs; - inst.typedefName = cls.typedefName; - inst.isVirtual = cls.isVirtual; + if (isVirtual) + wrapperFile.oss << "\n" + "void " << upcastFromVoidFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n" + " mexAtExit(&_deleteAllObjects);\n" + " typedef boost::shared_ptr<" << cppName + << "> Shared;\n" + " boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0]));\n" + " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n" + " Shared *self = new Shared(boost::static_pointer_cast<" << cppName + << ">(*asVoid));\n" + " *reinterpret_cast(mxGetData(out[0])) = self;\n" + "}\n"; +} + +/* ************************************************************************* */ +vector expandArgumentListsTemplate( + const vector& argLists, const string& templateArg, + const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + vector result; + BOOST_FOREACH(const ArgumentList& argList, argLists) { + ArgumentList instArgList; + BOOST_FOREACH(const Argument& arg, argList) { + Argument instArg = arg; + if (arg.type == templateArg) { + instArg.namespaces.assign(instName.begin(), instName.end() - 1); + instArg.type = instName.back(); + } else if (arg.type == "This") { + instArg.namespaces.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instArg.type = expandedClassName; + } + instArgList.push_back(instArg); + } + result.push_back(instArgList); + } + return result; +} + +/* ************************************************************************* */ +template +map expandMethodTemplate(const map& methods, + const string& templateArg, const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + map result; + typedef pair Name_Method; + BOOST_FOREACH(const Name_Method& name_method, methods) { + const METHOD& method = name_method.second; + METHOD instMethod = method; + instMethod.argLists = expandArgumentListsTemplate(method.argLists, + templateArg, instName, expandedClassNamespace, expandedClassName); + instMethod.returnVals.clear(); + BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { + ReturnValue instRetVal = retVal; + if (retVal.type1 == templateArg) { + instRetVal.namespaces1.assign(instName.begin(), instName.end() - 1); + instRetVal.type1 = instName.back(); + } else if (retVal.type1 == "This") { + instRetVal.namespaces1.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instRetVal.type1 = expandedClassName; + } + if (retVal.type2 == templateArg) { + instRetVal.namespaces2.assign(instName.begin(), instName.end() - 1); + instRetVal.type2 = instName.back(); + } else if (retVal.type1 == "This") { + instRetVal.namespaces2.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instRetVal.type2 = expandedClassName; + } + instMethod.returnVals.push_back(instRetVal); + } + result.insert(make_pair(name_method.first, instMethod)); + } + return result; +} + +/* ************************************************************************* */ +Class expandClassTemplate(const Class& cls, const string& templateArg, + const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + Class inst; + inst.name = cls.name; + inst.templateArgs = cls.templateArgs; + inst.typedefName = cls.typedefName; + inst.isVirtual = cls.isVirtual; inst.isSerializable = cls.isSerializable; - inst.qualifiedParent = cls.qualifiedParent; - inst.methods = expandMethodTemplate(cls.methods, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.namespaces = cls.namespaces; - inst.constructor = cls.constructor; - inst.constructor.args_list = expandArgumentListsTemplate(cls.constructor.args_list, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.constructor.name = inst.name; - inst.deconstructor = cls.deconstructor; - inst.deconstructor.name = inst.name; - inst.verbose_ = cls.verbose_; - return inst; -} - -/* ************************************************************************* */ -vector Class::expandTemplate(const string& templateArg, const vector >& instantiations) const { - vector result; - BOOST_FOREACH(const vector& instName, instantiations) { - const string expandedName = name + instName.back(); - Class inst = expandClassTemplate(*this, templateArg, instName, this->namespaces, expandedName); - inst.name = expandedName; - inst.templateArgs.clear(); - inst.typedefName = qualifiedName("::") + "<" + wrap::qualifiedName("::", instName) + ">"; - result.push_back(inst); - } - return result; -} - -/* ************************************************************************* */ -Class Class::expandTemplate(const string& templateArg, const vector& instantiation, const std::vector& expandedClassNamespace, const string& expandedClassName) const { - return expandClassTemplate(*this, templateArg, instantiation, expandedClassNamespace, expandedClassName); -} - -/* ************************************************************************* */ -std::string Class::getTypedef() const { - string result; - BOOST_FOREACH(const string& namesp, namespaces) { - result += ("namespace " + namesp + " { "); - } - result += ("typedef " + typedefName + " " + name + ";"); - for (size_t i = 0; i Class::expandTemplate(const string& templateArg, + const vector >& instantiations) const { + vector result; + BOOST_FOREACH(const vector& instName, instantiations) { + const string expandedName = name + instName.back(); + Class inst = expandClassTemplate(*this, templateArg, instName, + this->namespaces, expandedName); + inst.name = expandedName; + inst.templateArgs.clear(); + inst.typedefName = qualifiedName("::") + "<" + + wrap::qualifiedName("::", instName) + ">"; + result.push_back(inst); + } + return result; +} + +/* ************************************************************************* */ +Class Class::expandTemplate(const string& templateArg, + const vector& instantiation, + const std::vector& expandedClassNamespace, + const string& expandedClassName) const { + return expandClassTemplate(*this, templateArg, instantiation, + expandedClassNamespace, expandedClassName); +} + +/* ************************************************************************* */ +std::string Class::getTypedef() const { + string result; + BOOST_FOREACH(const string& namesp, namespaces) { + result += ("namespace " + namesp + " { "); + } + result += ("typedef " + typedefName + " " + name + ";"); + for (size_t i = 0; i < namespaces.size(); ++i) { + result += " }"; + } + return result; +} + +/* ************************************************************************* */ void Class::comment_fragment(FileWriter& proxyFile) const { - proxyFile.oss << "%class " << name - << ", see Doxygen page for details\n"; + proxyFile.oss << "%class " << name << ", see Doxygen page for details\n"; proxyFile.oss << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; @@ -396,15 +445,17 @@ void Class::comment_fragment(FileWriter& proxyFile) const { if (hasSerialization) { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; proxyFile.oss << "%string_serialize() : returns string\n"; - proxyFile.oss << "%string_deserialize(string serialized) : returns " << this->name << "\n"; + proxyFile.oss << "%string_deserialize(string serialized) : returns " + << this->name << "\n"; } proxyFile.oss << "%\n"; } -/* ************************************************************************* */ -void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const { +/* ************************************************************************* */ +void Class::serialization_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const std::string& wrapperName, + std::vector& functionNames) const { //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ @@ -418,30 +469,34 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi //} int serialize_id = functionNames.size(); - const string - matlabQualName = qualifiedName("."), - matlabUniqueName = qualifiedName(), - cppClassName = qualifiedName("::"); - const string wrapFunctionNameSerialize = matlabUniqueName + "_string_serialize_" + boost::lexical_cast(serialize_id); + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppClassName = qualifiedName("::"); + const string wrapFunctionNameSerialize = matlabUniqueName + + "_string_serialize_" + boost::lexical_cast(serialize_id); functionNames.push_back(wrapFunctionNameSerialize); // call //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) - wrapperFile.oss << "void " << wrapFunctionNameSerialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "void " << wrapFunctionNameSerialize + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; wrapperFile.oss << "{\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; // check arguments - for serialize, no arguments // example: checkArguments("string_serialize",nargout,nargin-1,0); - wrapperFile.oss << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n"; + wrapperFile.oss + << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n"; // get class pointer // example: Shared obj = unwrap_shared_ptr(in[0], "ptr_Point3"); - wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; + wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // Serialization boilerplate wrapperFile.oss << " std::ostringstream out_archive_stream;\n"; - wrapperFile.oss << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; + wrapperFile.oss + << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; wrapperFile.oss << " out_archive << *obj;\n"; wrapperFile.oss << " out[0] = wrap< string >(out_archive_stream.str());\n"; @@ -459,13 +514,19 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi // end // end - proxyFile.oss << " function varargout = string_serialize(this, varargin)\n"; - proxyFile.oss << " % STRING_SERIALIZE usage: string_serialize() : returns string\n"; - proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; + proxyFile.oss + << " function varargout = string_serialize(this, varargin)\n"; + proxyFile.oss + << " % STRING_SERIALIZE usage: string_serialize() : returns string\n"; + proxyFile.oss + << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; proxyFile.oss << " if length(varargin) == 0\n"; - proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast(serialize_id) << ", this, varargin{:});\n"; + proxyFile.oss << " varargout{1} = " << wrapperName << "(" + << boost::lexical_cast(serialize_id) << ", this, varargin{:});\n"; proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_serialize');\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << ".string_serialize');\n"; proxyFile.oss << " end\n"; proxyFile.oss << " end\n\n"; @@ -476,14 +537,16 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi // end proxyFile.oss << " function sobj = saveobj(obj)\n"; - proxyFile.oss << " % SAVEOBJ Saves the object to a matlab-readable format\n"; + proxyFile.oss + << " % SAVEOBJ Saves the object to a matlab-readable format\n"; proxyFile.oss << " sobj = obj.string_serialize();\n"; proxyFile.oss << " end\n"; } /* ************************************************************************* */ -void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const { +void Class::deserialization_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const std::string& wrapperName, + std::vector& functionNames) const { //void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ // typedef boost::shared_ptr Shared; @@ -495,32 +558,36 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper // in_archive >> *output; // out[0] = wrap_shared_ptr(output,"Point3", false); //} - int deserialize_id = functionNames.size(); - const string - matlabQualName = qualifiedName("."), - matlabUniqueName = qualifiedName(), - cppClassName = qualifiedName("::"); - const string wrapFunctionNameDeserialize = matlabUniqueName + "_string_deserialize_" + boost::lexical_cast(deserialize_id); - functionNames.push_back(wrapFunctionNameDeserialize); + int deserialize_id = functionNames.size(); + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppClassName = qualifiedName("::"); + const string wrapFunctionNameDeserialize = matlabUniqueName + + "_string_deserialize_" + boost::lexical_cast(deserialize_id); + functionNames.push_back(wrapFunctionNameDeserialize); - // call - wrapperFile.oss << "void " << wrapFunctionNameDeserialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - wrapperFile.oss << "{\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + // call + wrapperFile.oss << "void " << wrapFunctionNameDeserialize + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "{\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; - // check arguments - for deserialize, 1 string argument - wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << ".string_deserialize\",nargout,nargin,1);\n"; + // check arguments - for deserialize, 1 string argument + wrapperFile.oss << " checkArguments(\"" << matlabUniqueName + << ".string_deserialize\",nargout,nargin,1);\n"; - // string argument with deserialization boilerplate - wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; - wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; - wrapperFile.oss << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; - wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; - wrapperFile.oss << " in_archive >> *output;\n"; - wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName << "\", false);\n"; - wrapperFile.oss << "}\n"; + // string argument with deserialization boilerplate + wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; + wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; + wrapperFile.oss + << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; + wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; + wrapperFile.oss << " in_archive >> *output;\n"; + wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName + << "\", false);\n"; + wrapperFile.oss << "}\n"; - // Generate matlab function + // Generate matlab function // function varargout = string_deserialize(varargin) // % STRING_DESERIALIZE usage: string_deserialize() : returns Point3 // % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html @@ -531,32 +598,40 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper // end // end - proxyFile.oss << " function varargout = string_deserialize(varargin)\n"; - proxyFile.oss << " % STRING_DESERIALIZE usage: string_deserialize() : returns " << matlabQualName << "\n"; - proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; - proxyFile.oss << " if length(varargin) == 1\n"; - proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast(deserialize_id) << ", varargin{:});\n"; - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_deserialize');\n"; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n\n"; + proxyFile.oss << " function varargout = string_deserialize(varargin)\n"; + proxyFile.oss + << " % STRING_DESERIALIZE usage: string_deserialize() : returns " + << matlabQualName << "\n"; + proxyFile.oss + << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; + proxyFile.oss << " if length(varargin) == 1\n"; + proxyFile.oss << " varargout{1} = " << wrapperName << "(" + << boost::lexical_cast(deserialize_id) << ", varargin{:});\n"; + proxyFile.oss << " else\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << ".string_deserialize');\n"; + proxyFile.oss << " end\n"; + proxyFile.oss << " end\n\n"; - - // Generate matlab load function + // Generate matlab load function // function obj = loadobj(sobj) // % LOADOBJ Saves the object to a matlab-readable format // obj = Point3.string_deserialize(sobj); // end - proxyFile.oss << " function obj = loadobj(sobj)\n"; - proxyFile.oss << " % LOADOBJ Saves the object to a matlab-readable format\n"; - proxyFile.oss << " obj = " << matlabQualName << ".string_deserialize(sobj);\n"; - proxyFile.oss << " end" << endl; + proxyFile.oss << " function obj = loadobj(sobj)\n"; + proxyFile.oss + << " % LOADOBJ Saves the object to a matlab-readable format\n"; + proxyFile.oss << " obj = " << matlabQualName + << ".string_deserialize(sobj);\n"; + proxyFile.oss << " end" << endl; } /* ************************************************************************* */ std::string Class::getSerializationExport() const { //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal"); - return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + qualifiedName() + "\");"; + return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + + qualifiedName() + "\");"; } /* ************************************************************************* */ diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index e5c10b4c8..8c84030e7 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -28,7 +28,6 @@ using namespace std; using namespace wrap; - /* ************************************************************************* */ void StaticMethod::addOverload(bool verbose, const std::string& name, const ArgumentList& args, const ReturnValue& retVal) { @@ -39,144 +38,149 @@ void StaticMethod::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const string& cppClassName, - const std::string& matlabQualName, - const std::string& matlabUniqueName, - const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { +void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const string& cppClassName, + const std::string& matlabQualName, const std::string& matlabUniqueName, + const string& wrapperName, const TypeAttributesTable& typeAttributes, + vector& functionNames) const { - string upperName = name; upperName[0] = std::toupper(upperName[0], std::locale()); + string upperName = name; + upperName[0] = std::toupper(upperName[0], std::locale()); proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; //Comments for documentation - string up_name = boost::to_upper_copy(name); + string up_name = boost::to_upper_copy(name); proxyFile.oss << " % " << up_name << " usage:"; unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - if(argLCount != argLists.size()-1) - proxyFile.oss << "), "; - else - proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; + BOOST_FOREACH(ArgumentList argList, argLists) { + proxyFile.oss << " " << name << "("; + unsigned int i = 0; + BOOST_FOREACH(const Argument& arg, argList) { + if (i != argList.size() - 1) + proxyFile.oss << arg.type << " " << arg.name << ", "; + else + proxyFile.oss << arg.type << " " << arg.name; + i++; } + if (argLCount != argLists.size() - 1) + proxyFile.oss << "), "; + else + proxyFile.oss << ") : returns " + << returnVals[0].return_type(false, returnVals[0].pair) << endl; + argLCount++; + } - proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; + proxyFile.oss << " % " + << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" + << endl; proxyFile.oss << " % " << "" << endl; proxyFile.oss << " % " << "Usage" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " % " << up_name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + proxyFile.oss << " % " << up_name << "("; + unsigned int i = 0; + BOOST_FOREACH(const Argument& arg, argList) { + if (i != argList.size() - 1) + proxyFile.oss << arg.type << " " << arg.name << ", "; + else + proxyFile.oss << arg.type << " " << arg.name; + i++; } + proxyFile.oss << ")" << endl; + } - - for(size_t overload = 0; overload < argLists.size(); ++overload) { + for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; size_t nrArgs = args.size(); - const int id = (int)functionNames.size(); + const int id = (int) functionNames.size(); // Output proxy matlab code // check for number of arguments... - proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) proxyFile.oss << " && "; + proxyFile.oss << " " << (overload == 0 ? "" : "else") + << "if length(varargin) == " << nrArgs; + if (nrArgs > 0) + proxyFile.oss << " && "; // ...and their types bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + + boost::lexical_cast(id); const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; // call - file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start file.oss << "{\n"; returnVal.wrapTypeUnwrap(file); - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" + << endl; // check arguments // NOTE: for static functions, there is no object passed - file.oss << " checkArguments(\"" << matlabUniqueName << "." << name << "\",nargout,nargin," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << matlabUniqueName << "." << name + << "\",nargout,nargin," << args.size() << ");\n"; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file,0); // We start at 0 because there is no self object + args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1!="void") - returnVal.wrap_result(cppClassName+"::"+name+"("+args.names()+")", file, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result(cppClassName + "::" + name + "(" + args.names() + ")", + file, typeAttributes); else - file.oss << cppClassName+"::"+name+"("+args.names()+");\n"; + file.oss << cppClassName + "::" + name + "(" + args.names() + ");\n"; // finish file.oss << "}\n"; From 80b7d91264bbe5fe0c2505fcaee0c4fe7fd82b5b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:01:36 -0400 Subject: [PATCH 39/70] emit prototype method (to eliminate horrible copy/paste mess someone thought was good programming style) --- wrap/Argument.cpp | 22 ++++++++++++++++++---- wrap/Argument.h | 7 +++++++ 2 files changed, 25 insertions(+), 4 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index c3798e5ce..0f5961a48 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -16,13 +16,14 @@ * @author Richard Roberts **/ -#include -#include -#include +#include "Argument.h" + #include #include -#include "Argument.h" +#include +#include +#include using namespace std; using namespace wrap; @@ -128,4 +129,17 @@ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { } /* ************************************************************************* */ +void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { + file.oss << name << "("; + unsigned int i = 0; + BOOST_FOREACH(Argument arg, *this) { + if (i != size() - 1) + file.oss << arg.type << " " << arg.name << ", "; + else + file.oss << arg.type << " " << arg.name; + i++; + } + file.oss << ")"; +} +/* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index f46eaa427..da15cda36 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -68,6 +68,13 @@ struct ArgumentList: public std::vector { */ void matlab_unwrap(FileWriter& file, int start = 0) const; // MATLAB to C++ + /** + * emit MATLAB prototype + * @param file output stream + * @param name of method or function + */ + void emit_prototype(FileWriter& file, const std::string& name) const; + }; } // \namespace wrap From 26cae48338c8b350f0c85d72dbd43fdc7c4fd1a8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:02:29 -0400 Subject: [PATCH 40/70] First successful use of new ArgumentList::emit_prototype method --- wrap/Class.cpp | 27 ++++++++++----------------- wrap/Class.h | 1 - 2 files changed, 10 insertions(+), 18 deletions(-) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 8788e740f..e8b26efb2 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -16,18 +16,19 @@ * @author Richard Roberts **/ +#include "Class.h" +#include "utilities.h" +#include "Argument.h" + +#include +#include + #include #include #include //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC -#include -#include - -#include "Class.h" -#include "utilities.h" -#include "Argument.h" using namespace std; using namespace wrap; @@ -406,17 +407,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { BOOST_FOREACH(const Methods::value_type& name_m, methods) { const Method& m = name_m.second; BOOST_FOREACH(ArgumentList argList, m.argLists) { - string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << m.name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ") : returns " + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, m.name); + proxyFile.oss << " : returns " << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; } } diff --git a/wrap/Class.h b/wrap/Class.h index f5267cee2..2ca976f66 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -27,7 +27,6 @@ #include "Method.h" #include "StaticMethod.h" #include "TypeAttributesTable.h" -#include namespace wrap { From 47e90166938a5c80b2562a464974454967aa45d8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:02:54 -0400 Subject: [PATCH 41/70] Fixed so prints expected file then actual, as expected (no pun intended) --- wrap/utilities.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 1acc50db1..870ba3101 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -89,7 +89,7 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) bool equal = actual_contents == expected_contents; if (!equal) { stringstream command; - command << "diff " << actual << " " << expected << endl; + command << "diff " << expected << " " << actual << endl; system(command.str().c_str()); } return equal; From ededc01ec149899bc3671b5bd1b9f90c23851659 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:03:05 -0400 Subject: [PATCH 42/70] moved target --- .cproject | 422 +++++++++++++++++++++++++++--------------------------- 1 file changed, 213 insertions(+), 209 deletions(-) diff --git a/.cproject b/.cproject index fa8c24875..da776f062 100644 --- a/.cproject +++ b/.cproject @@ -1,17 +1,19 @@ - + + + + + - - @@ -60,13 +62,13 @@ + + - - @@ -116,13 +118,13 @@ + + - - @@ -540,14 +542,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -574,6 +568,7 @@ make + tests/testBayesTree.run true false @@ -581,6 +576,7 @@ make + testBinaryBayesNet.run true false @@ -628,6 +624,7 @@ make + testSymbolicBayesNet.run true false @@ -635,6 +632,7 @@ make + tests/testSymbolicFactor.run true false @@ -642,6 +640,7 @@ make + testSymbolicFactorGraph.run true false @@ -657,11 +656,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -886,22 +894,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -918,6 +910,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -942,6 +950,30 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -1006,30 +1038,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -1118,10 +1126,10 @@ true true - + make -j5 - testSmartProjectionPoseFactor.run + testWrap.run true true true @@ -1254,14 +1262,6 @@ true true - - make - -j5 - testAttitudeFactor.run - true - true - true - make -j5 @@ -1318,6 +1318,14 @@ true true + + make + -j5 + testSmartProjectionFactor.run + true + true + true + make -j5 @@ -1536,6 +1544,7 @@ make + testGraph.run true false @@ -1543,6 +1552,7 @@ make + testJunctionTree.run true false @@ -1550,6 +1560,7 @@ make + testSymbolicBayesNetB.run true false @@ -1717,6 +1728,7 @@ make + testErrors.run true false @@ -1762,22 +1774,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1858,6 +1854,22 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -2010,22 +2022,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j2 @@ -2108,7 +2104,6 @@ make - testSimulated2DOriented.run true false @@ -2148,7 +2143,6 @@ make - testSimulated2D.run true false @@ -2156,7 +2150,6 @@ make - testSimulated3D.run true false @@ -2170,6 +2163,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j5 @@ -2396,7 +2405,6 @@ make - tests/testGaussianISAM2 true false @@ -2418,102 +2426,6 @@ 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 @@ -2715,6 +2627,7 @@ cpack + -G DEB true false @@ -2722,6 +2635,7 @@ cpack + -G RPM true false @@ -2729,6 +2643,7 @@ cpack + -G TGZ true false @@ -2736,6 +2651,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2909,34 +2825,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -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 @@ -2980,6 +2960,30 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + From 4403d47865566abda9cb2b5e1a571abc4e1029f1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:12:48 -0400 Subject: [PATCH 43/70] Slight refactor --- wrap/Argument.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 0f5961a48..689ca6d88 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -131,13 +131,11 @@ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { /* ************************************************************************* */ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { file.oss << name << "("; - unsigned int i = 0; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (i != size() - 1) - file.oss << arg.type << " " << arg.name << ", "; - else - file.oss << arg.type << " " << arg.name; - i++; + if (!first) file.oss << ", "; + file.oss << arg.type << " " << arg.name; + first = false; } file.oss << ")"; } From 95b85e8494671aea570a18816858e440cea98d5b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:22:10 -0400 Subject: [PATCH 44/70] Now using ArgumentList::emit_prototype everywhere, for non copy/paste code --- wrap/Class.cpp | 30 ++++++---------------------- wrap/Method.cpp | 46 ++++++++++++++++++------------------------- wrap/Method.h | 7 +++---- wrap/StaticMethod.cpp | 38 +++++++++++------------------------ wrap/StaticMethod.h | 7 +++---- 5 files changed, 43 insertions(+), 85 deletions(-) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index e8b26efb2..075c98811 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -29,7 +29,6 @@ //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC - using namespace std; using namespace wrap; @@ -389,17 +388,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { if (!constructor.args_list.empty()) proxyFile.oss << "%\n%-------Constructors-------\n"; BOOST_FOREACH(ArgumentList argList, constructor.args_list) { - string up_name = boost::to_upper_copy(name); - proxyFile.oss << "%" << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")\n"; + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, name); + proxyFile.oss << "\n"; } if (!methods.empty()) @@ -419,18 +410,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { const StaticMethod& m = name_m.second; BOOST_FOREACH(ArgumentList argList, m.argLists) { - string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << m.name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - - proxyFile.oss << ") : returns " + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, m.name); + proxyFile.oss << " : returns " << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; } } diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 3fbf6a9c0..edfb80268 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -15,14 +15,15 @@ * @author Richard Roberts **/ -#include -#include +#include "Method.h" +#include "utilities.h" #include #include +#include -#include "Method.h" -#include "utilities.h" +#include +#include using namespace std; using namespace wrap; @@ -44,47 +45,38 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, const string& wrapperName, const TypeAttributesTable& typeAttributes, vector& functionNames) const { + // Create function header proxyFile.oss << " function varargout = " << name << "(this, varargin)\n"; - //Comments for documentation + + // Emit comments for documentation string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage:"; + proxyFile.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } + argList.emit_prototype(proxyFile, name); if (argLCount != argLists.size() - 1) - proxyFile.oss << "), "; + proxyFile.oss << ", "; else - proxyFile.oss << ") : returns " + proxyFile.oss << " : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; argLCount++; } + // Emit URL to Doxygen page proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; + + // Document all overloads, if any proxyFile.oss << " % " << "" << endl; proxyFile.oss << " % " << "Method Overloads" << endl; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " % " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; + proxyFile.oss << " % "; + argList.emit_prototype(proxyFile, name); + proxyFile.oss << endl; } + // For all overloads, check the number of arguments for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; diff --git a/wrap/Method.h b/wrap/Method.h index f2a7ed190..9926a5179 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -18,13 +18,12 @@ #pragma once -#include -#include - #include "Argument.h" #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include + +#include +#include namespace wrap { diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 8c84030e7..29b3b12df 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -16,14 +16,15 @@ * @author Richard Roberts **/ -#include -#include +#include "StaticMethod.h" +#include "utilities.h" #include #include +#include -#include "StaticMethod.h" -#include "utilities.h" +#include +#include using namespace std; using namespace wrap; @@ -50,22 +51,14 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; //Comments for documentation string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage:"; + proxyFile.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } + argList.emit_prototype(proxyFile, name); if (argLCount != argLists.size() - 1) - proxyFile.oss << "), "; + proxyFile.oss << ", "; else - proxyFile.oss << ") : returns " + proxyFile.oss << " : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; argLCount++; } @@ -76,16 +69,9 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, proxyFile.oss << " % " << "" << endl; proxyFile.oss << " % " << "Usage" << endl; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " % " << up_name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; + proxyFile.oss << " % "; + argList.emit_prototype(proxyFile, up_name); + proxyFile.oss << endl; } for (size_t overload = 0; overload < argLists.size(); ++overload) { diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 3e8dc08cf..576232346 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -19,13 +19,12 @@ #pragma once -#include -#include - #include "Argument.h" #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include + +#include +#include namespace wrap { From 43e238c6ab4f4a10b2e50918862d517154ecfbfb Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:26:14 -0400 Subject: [PATCH 45/70] Formatting and headers --- wrap/Constructor.h | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 96d1030a1..5438c515c 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -18,11 +18,11 @@ #pragma once +#include "Argument.h" + #include #include -#include "Argument.h" - namespace wrap { // Constructor class @@ -34,7 +34,7 @@ struct Constructor { } // Then the instance variables are set directly by the Module constructor - std::vector args_list; + std::vector args_list; std::string name; bool verbose_; @@ -50,21 +50,18 @@ struct Constructor { * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end */ void proxy_fragment(FileWriter& file, const std::string& wrapperName, - bool hasParent, const int id, const ArgumentList args) const; + bool hasParent, const int id, const ArgumentList args) const; /// cpp wrapper std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - const std::string& cppBaseClassName, - int id, - const ArgumentList& al) const; + const std::string& cppClassName, const std::string& matlabUniqueName, + const std::string& cppBaseClassName, int id, + const ArgumentList& al) const; /// constructor function void generate_construct(FileWriter& file, const std::string& cppClassName, - std::vector& args_list) const; - + std::vector& args_list) const; + }; - } // \namespace wrap From 1002696f83176c510a7eaef3a04312a29fc61c3a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:29:06 -0400 Subject: [PATCH 46/70] Formatting and headers --- wrap/Argument.h | 4 ++-- wrap/ReturnValue.h | 21 +++++++++------------ wrap/StaticMethod.h | 22 +++++++++------------- 3 files changed, 20 insertions(+), 27 deletions(-) diff --git a/wrap/Argument.h b/wrap/Argument.h index da15cda36..2a989713b 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -19,11 +19,11 @@ #pragma once +#include "FileWriter.h" + #include #include -#include "FileWriter.h" - namespace wrap { /// Argument class diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 7a677532f..ea78923f6 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -8,12 +8,11 @@ * @author Richard Roberts */ -#include -#include - #include "FileWriter.h" #include "TypeAttributesTable.h" +#include + #pragma once namespace wrap { @@ -21,16 +20,13 @@ namespace wrap { struct ReturnValue { typedef enum { - CLASS = 1, - EIGEN = 2, - BASIS = 3, - VOID = 4 + CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 } return_category; - ReturnValue(bool enable_verbosity = true) - : verbose(enable_verbosity), isPtr1(false), isPtr2(false), - isPair(false), category1(CLASS), category2(CLASS) - {} + ReturnValue(bool enable_verbosity = true) : + verbose(enable_verbosity), isPtr1(false), isPtr2(false), isPair(false), category1( + CLASS), category2(CLASS) { + } bool verbose; bool isPtr1, isPtr2, isPair; @@ -49,7 +45,8 @@ struct ReturnValue { std::string matlab_returnType() const; - void wrap_result(const std::string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const; + void wrap_result(const std::string& result, FileWriter& file, + const TypeAttributesTable& typeAttributes) const; void wrapTypeUnwrap(FileWriter& wrapperFile) const; diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 576232346..e1855f4c2 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -23,9 +23,6 @@ #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include -#include - namespace wrap { /// StaticMethod class @@ -33,7 +30,8 @@ struct StaticMethod { /// Constructor creates empty object StaticMethod(bool verbosity = true) : - verbose(verbosity) {} + verbose(verbosity) { + } // Then the instance variables are set directly by the Module constructor bool verbose; @@ -45,22 +43,20 @@ struct StaticMethod { // with those in rhs, but in subsequent calls it adds additional argument // lists as function overloads. void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal); + const ArgumentList& args, const ReturnValue& retVal); // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName, - const std::string& wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; + const std::string& cppClassName, const std::string& matlabQualName, + const std::string& matlabUniqueName, const std::string& wrapperName, + const TypeAttributesTable& typeAttributes, + std::vector& functionNames) const; private: std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - int overload, - int id, - const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + const std::string& cppClassName, const std::string& matlabUniqueName, + int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper }; } // \namespace wrap From 61baef3be0c6875fb70bb27946425ef1ac588566 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:37:44 -0400 Subject: [PATCH 47/70] Don't emit overloads unless there are any --- wrap/Method.cpp | 14 ++--- wrap/tests/expected/Point2.m | 21 -------- wrap/tests/expected/Point3.m | 3 -- wrap/tests/expected/Test.m | 57 -------------------- wrap/tests/expected_namespaces/+ns2/ClassA.m | 9 ---- 5 files changed, 8 insertions(+), 96 deletions(-) diff --git a/wrap/Method.cpp b/wrap/Method.cpp index edfb80268..ae50a36ae 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -68,12 +68,14 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, << endl; // Document all overloads, if any - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Method Overloads" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " % "; - argList.emit_prototype(proxyFile, name); - proxyFile.oss << endl; + if (argLists.size() > 1) { + proxyFile.oss << " % " << "" << endl; + proxyFile.oss << " % " << "Method Overloads" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + proxyFile.oss << " % "; + argList.emit_prototype(proxyFile, name); + proxyFile.oss << endl; + } } // For all overloads, check the number of arguments diff --git a/wrap/tests/expected/Point2.m b/wrap/tests/expected/Point2.m index 22dec9641..fa0741fe2 100644 --- a/wrap/tests/expected/Point2.m +++ b/wrap/tests/expected/Point2.m @@ -44,9 +44,6 @@ classdef Point2 < handle function varargout = argChar(this, varargin) % ARGCHAR usage: argChar(char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % argChar(char a) if length(varargin) == 1 && isa(varargin{1},'char') geometry_wrapper(4, this, varargin{:}); else @@ -57,9 +54,6 @@ classdef Point2 < handle function varargout = argUChar(this, varargin) % ARGUCHAR usage: argUChar(unsigned char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % argUChar(unsigned char a) if length(varargin) == 1 && isa(varargin{1},'char') geometry_wrapper(5, this, varargin{:}); else @@ -70,9 +64,6 @@ classdef Point2 < handle function varargout = dim(this, varargin) % DIM usage: dim() : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % dim() if length(varargin) == 0 varargout{1} = geometry_wrapper(6, this, varargin{:}); else @@ -83,9 +74,6 @@ classdef Point2 < handle function varargout = returnChar(this, varargin) % RETURNCHAR usage: returnChar() : returns char % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % returnChar() if length(varargin) == 0 varargout{1} = geometry_wrapper(7, this, varargin{:}); else @@ -96,9 +84,6 @@ classdef Point2 < handle function varargout = vectorConfusion(this, varargin) % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % vectorConfusion() if length(varargin) == 0 varargout{1} = geometry_wrapper(8, this, varargin{:}); else @@ -109,9 +94,6 @@ classdef Point2 < handle function varargout = x(this, varargin) % X usage: x() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % x() if length(varargin) == 0 varargout{1} = geometry_wrapper(9, this, varargin{:}); else @@ -122,9 +104,6 @@ classdef Point2 < handle function varargout = y(this, varargin) % Y usage: y() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % y() if length(varargin) == 0 varargout{1} = geometry_wrapper(10, this, varargin{:}); else diff --git a/wrap/tests/expected/Point3.m b/wrap/tests/expected/Point3.m index 94d9c25b8..fd17df60b 100644 --- a/wrap/tests/expected/Point3.m +++ b/wrap/tests/expected/Point3.m @@ -43,9 +43,6 @@ classdef Point3 < handle function varargout = norm(this, varargin) % NORM usage: norm() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % norm() if length(varargin) == 0 varargout{1} = geometry_wrapper(14, this, varargin{:}); else diff --git a/wrap/tests/expected/Test.m b/wrap/tests/expected/Test.m index 8e56df6fc..a82ae3ed3 100644 --- a/wrap/tests/expected/Test.m +++ b/wrap/tests/expected/Test.m @@ -56,9 +56,6 @@ classdef Test < handle function varargout = arg_EigenConstRef(this, varargin) % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % arg_EigenConstRef(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') geometry_wrapper(23, this, varargin{:}); else @@ -69,9 +66,6 @@ classdef Test < handle function varargout = create_MixedPtrs(this, varargin) % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % create_MixedPtrs() if length(varargin) == 0 [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); else @@ -82,9 +76,6 @@ classdef Test < handle function varargout = create_ptrs(this, varargin) % CREATE_PTRS usage: create_ptrs() : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % create_ptrs() if length(varargin) == 0 [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); else @@ -95,9 +86,6 @@ classdef Test < handle function varargout = print(this, varargin) % PRINT usage: print() : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % print() if length(varargin) == 0 geometry_wrapper(26, this, varargin{:}); else @@ -108,9 +96,6 @@ classdef Test < handle function varargout = return_Point2Ptr(this, varargin) % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2 % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_Point2Ptr(bool value) if length(varargin) == 1 && isa(varargin{1},'logical') varargout{1} = geometry_wrapper(27, this, varargin{:}); else @@ -121,9 +106,6 @@ classdef Test < handle function varargout = return_Test(this, varargin) % RETURN_TEST usage: return_Test(Test value) : returns Test % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_Test(Test value) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(28, this, varargin{:}); else @@ -134,9 +116,6 @@ classdef Test < handle function varargout = return_TestPtr(this, varargin) % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_TestPtr(Test value) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(29, this, varargin{:}); else @@ -147,9 +126,6 @@ classdef Test < handle function varargout = return_bool(this, varargin) % RETURN_BOOL usage: return_bool(bool value) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_bool(bool value) if length(varargin) == 1 && isa(varargin{1},'logical') varargout{1} = geometry_wrapper(30, this, varargin{:}); else @@ -160,9 +136,6 @@ classdef Test < handle function varargout = return_double(this, varargin) % RETURN_DOUBLE usage: return_double(double value) : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_double(double value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(31, this, varargin{:}); else @@ -173,9 +146,6 @@ classdef Test < handle function varargout = return_field(this, varargin) % RETURN_FIELD usage: return_field(Test t) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_field(Test t) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(32, this, varargin{:}); else @@ -186,9 +156,6 @@ classdef Test < handle function varargout = return_int(this, varargin) % RETURN_INT usage: return_int(int value) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_int(int value) if length(varargin) == 1 && isa(varargin{1},'numeric') varargout{1} = geometry_wrapper(33, this, varargin{:}); else @@ -199,9 +166,6 @@ classdef Test < handle function varargout = return_matrix1(this, varargin) % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_matrix1(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(34, this, varargin{:}); else @@ -212,9 +176,6 @@ classdef Test < handle function varargout = return_matrix2(this, varargin) % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_matrix2(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(35, this, varargin{:}); else @@ -225,9 +186,6 @@ classdef Test < handle function varargout = return_pair(this, varargin) % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_pair(Vector v, Matrix A) if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') [ varargout{1} varargout{2} ] = geometry_wrapper(36, this, varargin{:}); else @@ -238,9 +196,6 @@ classdef Test < handle function varargout = return_ptrs(this, varargin) % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_ptrs(Test p1, Test p2) if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test') [ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:}); else @@ -251,9 +206,6 @@ classdef Test < handle function varargout = return_size_t(this, varargin) % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_size_t(size_t value) if length(varargin) == 1 && isa(varargin{1},'numeric') varargout{1} = geometry_wrapper(38, this, varargin{:}); else @@ -264,9 +216,6 @@ classdef Test < handle function varargout = return_string(this, varargin) % RETURN_STRING usage: return_string(string value) : returns string % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_string(string value) if length(varargin) == 1 && isa(varargin{1},'char') varargout{1} = geometry_wrapper(39, this, varargin{:}); else @@ -277,9 +226,6 @@ classdef Test < handle function varargout = return_vector1(this, varargin) % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_vector1(Vector value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(40, this, varargin{:}); else @@ -290,9 +236,6 @@ classdef Test < handle function varargout = return_vector2(this, varargin) % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_vector2(Vector value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(41, this, varargin{:}); else diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m index c7ab901ae..f31825851 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m @@ -40,9 +40,6 @@ classdef ClassA < handle function varargout = memberFunction(this, varargin) % MEMBERFUNCTION usage: memberFunction() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % memberFunction() if length(varargin) == 0 varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); else @@ -53,9 +50,6 @@ classdef ClassA < handle function varargout = nsArg(this, varargin) % NSARG usage: nsArg(ClassB arg) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % nsArg(ClassB arg) if length(varargin) == 1 && isa(varargin{1},'ns1.ClassB') varargout{1} = testNamespaces_wrapper(10, this, varargin{:}); else @@ -66,9 +60,6 @@ classdef ClassA < handle function varargout = nsReturn(this, varargin) % NSRETURN usage: nsReturn(double q) : returns ns2::ns3::ClassB % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % nsReturn(double q) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); else From 406419317fb7a119bc497c0a6ca6b4e325e0f85b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 14:35:07 -0400 Subject: [PATCH 48/70] eliminated more copy/paste mess between Method and StaticMethod --- wrap/Argument.cpp | 85 ++++++++++++++++++++++++++++++++----------- wrap/Argument.h | 9 +++++ wrap/Method.cpp | 80 ++++++++++++---------------------------- wrap/StaticMethod.cpp | 73 +++++++++++-------------------------- 4 files changed, 118 insertions(+), 129 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 689ca6d88..2ee705c5b 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -32,14 +32,14 @@ using namespace wrap; string Argument::matlabClass(const string& delim) const { string result; BOOST_FOREACH(const string& ns, namespaces) - result += ns + delim; - if (type=="string" || type=="unsigned char" || type=="char") + result += ns + delim; + if (type == "string" || type == "unsigned char" || type == "char") return result + "char"; - if (type=="Vector" || type=="Matrix") + if (type == "Vector" || type == "Matrix") return result + "double"; - if (type=="int" || type=="size_t") + if (type == "int" || type == "size_t") return result + "numeric"; - if (type=="bool") + if (type == "bool") return result + "logical"; return result + type; } @@ -53,7 +53,8 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { if (is_ptr) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer - file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; + file.oss << "boost::shared_ptr<" << cppType << "> " << name + << " = unwrap_shared_ptr< "; else if (is_ref) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; @@ -66,23 +67,28 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; - if (is_ptr || is_ref) file.oss << ", \"ptr_" << matlabUniqueType << "\""; + if (is_ptr || is_ref) + file.oss << ", \"ptr_" << matlabUniqueType << "\""; file.oss << ");" << endl; } /* ************************************************************************* */ string Argument::qualifiedType(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, namespaces) result += ns + delim; + BOOST_FOREACH(const string& ns, namespaces) + result += ns + delim; return result + type; } /* ************************************************************************* */ string ArgumentList::types() const { string str; - bool first=true; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) str += ","; str += arg.type; first=false; + if (!first) + str += ","; + str += arg.type; + first = false; } return str; } @@ -90,16 +96,17 @@ string ArgumentList::types() const { /* ************************************************************************* */ string ArgumentList::signature() const { string sig; - bool cap=false; + bool cap = false; BOOST_FOREACH(Argument arg, *this) { BOOST_FOREACH(char ch, arg.type) - if(isupper(ch)) { - sig += ch; - //If there is a capital letter, we don't want to read it below - cap=true; - } - if(!cap) sig += arg.type[0]; + if (isupper(ch)) { + sig += ch; + //If there is a capital letter, we don't want to read it below + cap = true; + } + if (!cap) + sig += arg.type[0]; //Reset to default cap = false; } @@ -110,9 +117,12 @@ string ArgumentList::signature() const { /* ************************************************************************* */ string ArgumentList::names() const { string str; - bool first=true; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) str += ","; str += arg.name; first=false; + if (!first) + str += ","; + str += arg.name; + first = false; } return str; } @@ -123,7 +133,7 @@ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { BOOST_FOREACH(Argument arg, *this) { stringstream buf; buf << "in[" << index << "]"; - arg.matlab_unwrap(file,buf.str()); + arg.matlab_unwrap(file, buf.str()); index++; } } @@ -133,11 +143,44 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { file.oss << name << "("; bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) file.oss << ", "; + if (!first) + file.oss << ", "; file.oss << arg.type << " " << arg.name; first = false; } file.oss << ")"; } /* ************************************************************************* */ +void ArgumentList::emit_conditional_call(FileWriter& file, + const ReturnValue& returnVal, const string& wrapperName, int id, + bool staticMethod) const { + // Check nr of arguments + file.oss << "if length(varargin) == " << size(); + if (size() > 0) + file.oss << " && "; + // ...and their types + bool first = true; + for (size_t i = 0; i < size(); i++) { + if (!first) + file.oss << " && "; + file.oss << "isa(varargin{" << i + 1 << "},'" << (*this)[i].matlabClass(".") + << "')"; + first = false; + } + file.oss << "\n"; + + // output call to C++ wrapper + string output; + if (returnVal.isPair) + output = "[ varargout{1} varargout{2} ] = "; + else if (returnVal.category1 == ReturnValue::VOID) + output = ""; + else + output = "varargout{1} = "; + file.oss << " " << output << wrapperName << "(" << id; + if (!staticMethod) + file.oss << ", this"; + file.oss << ", varargin{:});\n"; +} +/* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index 2a989713b..da6a733bb 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -20,6 +20,7 @@ #pragma once #include "FileWriter.h" +#include "ReturnValue.h" #include #include @@ -75,6 +76,14 @@ struct ArgumentList: public std::vector { */ void emit_prototype(FileWriter& file, const std::string& name) const; + /** + * emit conditional MATLAB call (checking arguments first) + * @param file output stream + * @param returnVal the return value + * @param wrapperName of method or function + */ + void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal, + const std::string& wrapperName, int id, bool staticMethod=false) const; }; } // \namespace wrap diff --git a/wrap/Method.cpp b/wrap/Method.cpp index ae50a36ae..7018ad138 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -39,99 +39,67 @@ void Method::addOverload(bool verbose, bool is_const, const std::string& name, } /* ************************************************************************* */ -void Method::proxy_wrapper_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, const string& cppClassName, - const std::string& matlabQualName, const std::string& matlabUniqueName, - const string& wrapperName, const TypeAttributesTable& typeAttributes, +void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, + const string& cppClassName, const std::string& matlabQualName, + const std::string& matlabUniqueName, const string& wrapperName, + const TypeAttributesTable& typeAttributes, vector& functionNames) const { // Create function header - proxyFile.oss << " function varargout = " << name << "(this, varargin)\n"; + file.oss << " function varargout = " << name << "(this, varargin)\n"; // Emit comments for documentation string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage: "; + file.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; BOOST_FOREACH(ArgumentList argList, argLists) { - argList.emit_prototype(proxyFile, name); + argList.emit_prototype(file, name); if (argLCount != argLists.size() - 1) - proxyFile.oss << ", "; + file.oss << ", "; else - proxyFile.oss << " : returns " + file.oss << " : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; argLCount++; } // Emit URL to Doxygen page - proxyFile.oss << " % " + file.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; // Document all overloads, if any if (argLists.size() > 1) { - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Method Overloads" << endl; + file.oss << " % " << "" << endl; + file.oss << " % " << "Method Overloads" << endl; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " % "; - argList.emit_prototype(proxyFile, name); - proxyFile.oss << endl; + file.oss << " % "; + argList.emit_prototype(file, name); + file.oss << endl; } } - // For all overloads, check the number of arguments + // Check arguments for all overloads for (size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); - - const int id = functionNames.size(); // Output proxy matlab code - - // check for number of arguments... - proxyFile.oss << " " << (overload == 0 ? "" : "else") - << "if length(varargin) == " << nrArgs; - if (nrArgs > 0) - proxyFile.oss << " && "; - // ...and their types - bool first = true; - for (size_t i = 0; i < nrArgs; i++) { - if (!first) - proxyFile.oss << " && "; - proxyFile.oss << "isa(varargin{" << i + 1 << "},'" - << args[i].matlabClass(".") << "')"; - first = false; - } - proxyFile.oss << "\n"; - - // output call to C++ wrapper - string output; - if (returnVal.isPair) - output = "[ varargout{1} varargout{2} ] = "; - else if (returnVal.category1 == ReturnValue::VOID) - output = ""; - else - output = "varargout{1} = "; - proxyFile.oss << " " << output << wrapperName << "(" << id - << ", this, varargin{:});\n"; + file.oss << " " << (overload == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + argLists[overload].emit_conditional_call(file, returnVals[overload], + wrapperName, id); // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, matlabUniqueName, overload, id, typeAttributes); // Add to function list functionNames.push_back(wrapFunctionName); - } - - proxyFile.oss << " else\n"; - proxyFile.oss - << " error('Arguments do not match any overload of function " + file.oss << " else\n"; + file.oss << " error('Arguments do not match any overload of function " << matlabQualName << "." << name << "');" << endl; + file.oss << " end\n"; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n"; + file.oss << " end\n"; } /* ************************************************************************* */ diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 29b3b12df..0c4cc7d75 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -39,7 +39,7 @@ void StaticMethod::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, +void StaticMethod::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, const string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName, const string& wrapperName, const TypeAttributesTable& typeAttributes, @@ -48,85 +48,54 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, string upperName = name; upperName[0] = std::toupper(upperName[0], std::locale()); - proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; + file.oss << " function varargout = " << upperName << "(varargin)\n"; //Comments for documentation string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage: "; + file.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; BOOST_FOREACH(ArgumentList argList, argLists) { - argList.emit_prototype(proxyFile, name); + argList.emit_prototype(file, name); if (argLCount != argLists.size() - 1) - proxyFile.oss << ", "; + file.oss << ", "; else - proxyFile.oss << " : returns " + file.oss << " : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; argLCount++; } - proxyFile.oss << " % " + file.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Usage" << endl; + file.oss << " % " << "" << endl; + file.oss << " % " << "Usage" << endl; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " % "; - argList.emit_prototype(proxyFile, up_name); - proxyFile.oss << endl; + file.oss << " % "; + argList.emit_prototype(file, up_name); + file.oss << endl; } + // Check arguments for all overloads for (size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); - - const int id = (int) functionNames.size(); // Output proxy matlab code - - // check for number of arguments... - proxyFile.oss << " " << (overload == 0 ? "" : "else") - << "if length(varargin) == " << nrArgs; - if (nrArgs > 0) - proxyFile.oss << " && "; - // ...and their types - bool first = true; - for (size_t i = 0; i < nrArgs; i++) { - if (!first) - proxyFile.oss << " && "; - proxyFile.oss << "isa(varargin{" << i + 1 << "},'" - << args[i].matlabClass(".") << "')"; - first = false; - } - proxyFile.oss << "\n"; - - // output call to C++ wrapper - string output; - if (returnVal.isPair) - output = "[ varargout{1} varargout{2} ] = "; - else if (returnVal.category1 == ReturnValue::VOID) - output = ""; - else - output = "varargout{1} = "; - proxyFile.oss << " " << output << wrapperName << "(" << id - << ", varargin{:});\n"; + file.oss << " " << (overload == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + argLists[overload].emit_conditional_call(file, returnVals[overload], + wrapperName, id, true); // last bool is to indicate static ! // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, matlabUniqueName, (int) overload, id, typeAttributes); // Add to function list functionNames.push_back(wrapFunctionName); - } - - proxyFile.oss << " else\n"; - proxyFile.oss - << " error('Arguments do not match any overload of function " + file.oss << " else\n"; + file.oss << " error('Arguments do not match any overload of function " << matlabQualName << "." << upperName << "');" << endl; + file.oss << " end\n"; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n"; + file.oss << " end\n"; } /* ************************************************************************* */ From 05008ecaa30d7a14d2110f69b750a33b55862312 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 14:40:39 -0400 Subject: [PATCH 49/70] Comments --- wrap/ReturnValue.h | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index ea78923f6..2bc5aa82b 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -17,23 +17,28 @@ namespace wrap { +/** + * Encapsulates return value of a method or function + */ struct ReturnValue { + /// the different supported return value categories typedef enum { CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 } return_category; - ReturnValue(bool enable_verbosity = true) : - verbose(enable_verbosity), isPtr1(false), isPtr2(false), isPair(false), category1( - CLASS), category2(CLASS) { - } - bool verbose; bool isPtr1, isPtr2, isPair; return_category category1, category2; std::string type1, type2; std::vector namespaces1, namespaces2; + /// Constructor + ReturnValue(bool enable_verbosity = true) : + verbose(enable_verbosity), isPtr1(false), isPtr2(false), isPair(false), category1( + CLASS), category2(CLASS) { + } + typedef enum { arg1, arg2, pair } pairing; From 52cd2007181d2561abefd6d4e4e95db5a9e5d6ad Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 14:52:49 -0400 Subject: [PATCH 50/70] ReturnValue now emits, eliminated some copy/paste. Also removed unused verbose field/argument in ReturnValue --- wrap/Argument.cpp | 11 +++-------- wrap/GlobalFunction.cpp | 11 +++-------- wrap/Method.cpp | 2 ++ wrap/Module.cpp | 2 +- wrap/ReturnValue.cpp | 8 ++++++++ wrap/ReturnValue.h | 8 ++++---- 6 files changed, 21 insertions(+), 21 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 2ee705c5b..1d7c43809 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -170,14 +170,9 @@ void ArgumentList::emit_conditional_call(FileWriter& file, file.oss << "\n"; // output call to C++ wrapper - string output; - if (returnVal.isPair) - output = "[ varargout{1} varargout{2} ] = "; - else if (returnVal.category1 == ReturnValue::VOID) - output = ""; - else - output = "varargout{1} = "; - file.oss << " " << output << wrapperName << "(" << id; + file.oss << " "; + returnVal.emit_matlab(file); + file.oss << wrapperName << "(" << id; if (!staticMethod) file.oss << ", this"; file.oss << ", varargin{:});\n"; diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 16f6d48f1..588cbf7c8 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -103,14 +103,9 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons mfunctionFile.oss << "\n"; // output call to C++ wrapper - string output; - if(returnVal.isPair) - output = "[ varargout{1} varargout{2} ] = "; - else if(returnVal.category1 == ReturnValue::VOID) - output = ""; - else - output = "varargout{1} = "; - mfunctionFile.oss << " " << output << wrapperName << "(" << id << ", varargin{:});\n"; + mfunctionFile.oss << " "; + returnVal.emit_matlab(mfunctionFile); + mfunctionFile.oss << wrapperName << "(" << id << ", varargin{:});\n"; // Output C++ wrapper code diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 7018ad138..c0ec79e91 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -78,6 +78,8 @@ void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, } } + // Handle special case of single overload with numeric + // Check arguments for all overloads for (size_t overload = 0; overload < argLists.size(); ++overload) { diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 6870d5178..2cb5ea7ed 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -98,7 +98,7 @@ void Module::parseMarkup(const std::string& data) { // The one with postfix 0 are used to reset the variables after parse. string methodName, methodName0; bool isConst, isConst0 = false; - ReturnValue retVal0(verbose), retVal(verbose); + ReturnValue retVal0, retVal; Argument arg0, arg; ArgumentList args0, args; vector arg_dup; ///keep track of duplicates diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 3cb68181b..78e36d4da 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -141,5 +141,13 @@ void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const { } } /* ************************************************************************* */ +void ReturnValue::emit_matlab(FileWriter& file) const { + string output; + if (isPair) + file.oss << "[ varargout{1} varargout{2} ] = "; + else if (category1 != ReturnValue::VOID) + file.oss << "varargout{1} = "; +} +/* ************************************************************************* */ diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 2bc5aa82b..989f81109 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -27,16 +27,15 @@ struct ReturnValue { CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 } return_category; - bool verbose; bool isPtr1, isPtr2, isPair; return_category category1, category2; std::string type1, type2; std::vector namespaces1, namespaces2; /// Constructor - ReturnValue(bool enable_verbosity = true) : - verbose(enable_verbosity), isPtr1(false), isPtr2(false), isPair(false), category1( - CLASS), category2(CLASS) { + ReturnValue() : + isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2( + CLASS) { } typedef enum { @@ -55,6 +54,7 @@ struct ReturnValue { void wrapTypeUnwrap(FileWriter& wrapperFile) const; + void emit_matlab(FileWriter& file) const; }; } // \namespace wrap From 9d9614d18552a627dc2949eeab7e3b652b26f1bd Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 14:59:20 -0400 Subject: [PATCH 51/70] Split up into two calls --- wrap/Argument.cpp | 15 ++++++++++----- wrap/Argument.h | 15 +++++++++++++-- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 1d7c43809..fb3d962ed 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -151,6 +151,15 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { file.oss << ")"; } /* ************************************************************************* */ +void ArgumentList::emit_call(FileWriter& file, const ReturnValue& returnVal, + const string& wrapperName, int id, bool staticMethod) const { + returnVal.emit_matlab(file); + file.oss << wrapperName << "(" << id; + if (!staticMethod) + file.oss << ", this"; + file.oss << ", varargin{:});\n"; +} +/* ************************************************************************* */ void ArgumentList::emit_conditional_call(FileWriter& file, const ReturnValue& returnVal, const string& wrapperName, int id, bool staticMethod) const { @@ -171,11 +180,7 @@ void ArgumentList::emit_conditional_call(FileWriter& file, // output call to C++ wrapper file.oss << " "; - returnVal.emit_matlab(file); - file.oss << wrapperName << "(" << id; - if (!staticMethod) - file.oss << ", this"; - file.oss << ", varargin{:});\n"; + emit_call(file, returnVal, wrapperName, id, staticMethod); } /* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index da6a733bb..9d57f2853 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -77,13 +77,24 @@ struct ArgumentList: public std::vector { void emit_prototype(FileWriter& file, const std::string& name) const; /** - * emit conditional MATLAB call (checking arguments first) + * emit emit MATLAB call to wrapper * @param file output stream * @param returnVal the return value * @param wrapperName of method or function + * @param staticMethod flag to emit "this" in call + */ + void emit_call(FileWriter& file, const ReturnValue& returnVal, + const std::string& wrapperName, int id, bool staticMethod = false) const; + + /** + * emit conditional MATLAB call to wrapper (checking arguments first) + * @param file output stream + * @param returnVal the return value + * @param wrapperName of method or function + * @param staticMethod flag to emit "this" in call */ void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal, - const std::string& wrapperName, int id, bool staticMethod=false) const; + const std::string& wrapperName, int id, bool staticMethod = false) const; }; } // \namespace wrap From 5987f78be34effcd5ba70fa303e3911c59e8459e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 15:21:13 -0400 Subject: [PATCH 52/70] Methods to check whether arguments are scalar --- wrap/Argument.cpp | 13 +++++++++++++ wrap/Argument.h | 6 ++++++ 2 files changed, 19 insertions(+) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index fb3d962ed..d76556e4a 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -44,6 +44,12 @@ string Argument::matlabClass(const string& delim) const { return result + type; } +/* ************************************************************************* */ +bool Argument::isScalar() const { + return (type == "bool" || type == "char" || type == "unsigned char" + || type == "int" || type == "size_t" || type == "double"); +} + /* ************************************************************************* */ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << " "; @@ -127,6 +133,13 @@ string ArgumentList::names() const { return str; } +/* ************************************************************************* */ +bool ArgumentList::allScalar() const { + BOOST_FOREACH(Argument arg, *this) + if (!arg.isScalar()) return false; + return true; +} + /* ************************************************************************* */ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { int index = start; diff --git a/wrap/Argument.h b/wrap/Argument.h index 9d57f2853..6f791978a 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -41,6 +41,9 @@ struct Argument { /// return MATLAB class for use in isa(x,class) std::string matlabClass(const std::string& delim = "") const; + /// Check if will be unwrapped using scalar login in wrap/matlab.h + bool isScalar() const; + /// adds namespaces to type std::string qualifiedType(const std::string& delim = "") const; @@ -60,6 +63,9 @@ struct ArgumentList: public std::vector { /// create a comma-separated string listing all argument names, used in m-files std::string names() const; + /// Check if all arguments scalar + bool allScalar() const; + // MATLAB code generation: /** From a38504dc6a615a2479f5a7f9c4331f058d510d78 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 15:21:33 -0400 Subject: [PATCH 53/70] Ignore some files --- wrap/tests/expected/.gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 wrap/tests/expected/.gitignore diff --git a/wrap/tests/expected/.gitignore b/wrap/tests/expected/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/wrap/tests/expected/.gitignore @@ -0,0 +1 @@ +*.m~ From 9a102e8c5976b7af629f3bf377fc17dbd0211ac4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 15:21:49 -0400 Subject: [PATCH 54/70] Handle special case of single overload with all numeric arguments --- wrap/Method.cpp | 40 ++++++++++------ wrap/tests/expected/Point2.m | 42 +++-------------- wrap/tests/expected/Point3.m | 6 +-- wrap/tests/expected/Test.m | 48 ++++---------------- wrap/tests/expected_namespaces/+ns2/ClassA.m | 12 +---- 5 files changed, 45 insertions(+), 103 deletions(-) diff --git a/wrap/Method.cpp b/wrap/Method.cpp index c0ec79e91..7b88b9cdc 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -78,28 +78,42 @@ void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, } } - // Handle special case of single overload with numeric - - // Check arguments for all overloads - for (size_t overload = 0; overload < argLists.size(); ++overload) { - + // Handle special case of single overload with all numeric arguments + if (argLists.size() == 1 && argLists[0].allScalar()) { // Output proxy matlab code - file.oss << " " << (overload == 0 ? "" : "else"); + file.oss << " "; const int id = (int) functionNames.size(); - argLists[overload].emit_conditional_call(file, returnVals[overload], - wrapperName, id); + argLists[0].emit_call(file, returnVals[0], wrapperName, id); // Output C++ wrapper code const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, - matlabUniqueName, overload, id, typeAttributes); + matlabUniqueName, 0, id, typeAttributes); // Add to function list functionNames.push_back(wrapFunctionName); + } else { + // Check arguments for all overloads + for (size_t overload = 0; overload < argLists.size(); ++overload) { + + // Output proxy matlab code + file.oss << " " << (overload == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + argLists[overload].emit_conditional_call(file, returnVals[overload], + wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, + cppClassName, matlabUniqueName, overload, id, typeAttributes); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } + file.oss << " else\n"; + file.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "." << name << "');" << endl; + file.oss << " end\n"; } - file.oss << " else\n"; - file.oss << " error('Arguments do not match any overload of function " - << matlabQualName << "." << name << "');" << endl; - file.oss << " end\n"; file.oss << " end\n"; } diff --git a/wrap/tests/expected/Point2.m b/wrap/tests/expected/Point2.m index fa0741fe2..9f64f2d10 100644 --- a/wrap/tests/expected/Point2.m +++ b/wrap/tests/expected/Point2.m @@ -44,71 +44,43 @@ classdef Point2 < handle function varargout = argChar(this, varargin) % ARGCHAR usage: argChar(char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'char') - geometry_wrapper(4, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.argChar'); - end + geometry_wrapper(4, this, varargin{:}); end function varargout = argUChar(this, varargin) % ARGUCHAR usage: argUChar(unsigned char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'char') - geometry_wrapper(5, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.argUChar'); - end + geometry_wrapper(5, this, varargin{:}); end function varargout = dim(this, varargin) % DIM usage: dim() : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(6, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.dim'); - end + varargout{1} = geometry_wrapper(6, this, varargin{:}); end function varargout = returnChar(this, varargin) % RETURNCHAR usage: returnChar() : returns char % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(7, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.returnChar'); - end + varargout{1} = geometry_wrapper(7, this, varargin{:}); end function varargout = vectorConfusion(this, varargin) % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(8, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.vectorConfusion'); - end + varargout{1} = geometry_wrapper(8, this, varargin{:}); end function varargout = x(this, varargin) % X usage: x() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(9, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.x'); - end + varargout{1} = geometry_wrapper(9, this, varargin{:}); end function varargout = y(this, varargin) % Y usage: y() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(10, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.y'); - end + varargout{1} = geometry_wrapper(10, this, varargin{:}); end end diff --git a/wrap/tests/expected/Point3.m b/wrap/tests/expected/Point3.m index fd17df60b..8a43987b9 100644 --- a/wrap/tests/expected/Point3.m +++ b/wrap/tests/expected/Point3.m @@ -43,11 +43,7 @@ classdef Point3 < handle function varargout = norm(this, varargin) % NORM usage: norm() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(14, this, varargin{:}); - else - error('Arguments do not match any overload of function Point3.norm'); - end + varargout{1} = geometry_wrapper(14, this, varargin{:}); end function varargout = string_serialize(this, varargin) diff --git a/wrap/tests/expected/Test.m b/wrap/tests/expected/Test.m index a82ae3ed3..1afd15efe 100644 --- a/wrap/tests/expected/Test.m +++ b/wrap/tests/expected/Test.m @@ -66,41 +66,25 @@ classdef Test < handle function varargout = create_MixedPtrs(this, varargin) % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.create_MixedPtrs'); - end + [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); end function varargout = create_ptrs(this, varargin) % CREATE_PTRS usage: create_ptrs() : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.create_ptrs'); - end + [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); end function varargout = print(this, varargin) % PRINT usage: print() : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - geometry_wrapper(26, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.print'); - end + geometry_wrapper(26, this, varargin{:}); end function varargout = return_Point2Ptr(this, varargin) % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2 % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = geometry_wrapper(27, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_Point2Ptr'); - end + varargout{1} = geometry_wrapper(27, this, varargin{:}); end function varargout = return_Test(this, varargin) @@ -126,21 +110,13 @@ classdef Test < handle function varargout = return_bool(this, varargin) % RETURN_BOOL usage: return_bool(bool value) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = geometry_wrapper(30, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_bool'); - end + varargout{1} = geometry_wrapper(30, this, varargin{:}); end function varargout = return_double(this, varargin) % RETURN_DOUBLE usage: return_double(double value) : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = geometry_wrapper(31, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_double'); - end + varargout{1} = geometry_wrapper(31, this, varargin{:}); end function varargout = return_field(this, varargin) @@ -156,11 +132,7 @@ classdef Test < handle function varargout = return_int(this, varargin) % RETURN_INT usage: return_int(int value) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(33, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_int'); - end + varargout{1} = geometry_wrapper(33, this, varargin{:}); end function varargout = return_matrix1(this, varargin) @@ -206,11 +178,7 @@ classdef Test < handle function varargout = return_size_t(this, varargin) % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(38, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_size_t'); - end + varargout{1} = geometry_wrapper(38, this, varargin{:}); end function varargout = return_string(this, varargin) diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m index f31825851..9c064734e 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m @@ -40,11 +40,7 @@ classdef ClassA < handle function varargout = memberFunction(this, varargin) % MEMBERFUNCTION usage: memberFunction() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.memberFunction'); - end + varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); end function varargout = nsArg(this, varargin) @@ -60,11 +56,7 @@ classdef ClassA < handle function varargout = nsReturn(this, varargin) % NSRETURN usage: nsReturn(double q) : returns ns2::ns3::ClassB % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.nsReturn'); - end + varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); end end From 05a38ca263cf6dbea6ab29cc060f99ea42c967c5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:01:30 -0400 Subject: [PATCH 55/70] Standard BORG formatting --- wrap/GlobalFunction.cpp | 80 ++++++++++++++++++++++------------------- wrap/GlobalFunction.h | 33 ++++++++--------- 2 files changed, 60 insertions(+), 53 deletions(-) diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 588cbf7c8..26d52018e 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -17,7 +17,8 @@ using namespace std; /* ************************************************************************* */ void GlobalFunction::addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack) { + const ArgumentList& args, const ReturnValue& retVal, + const StrVec& ns_stack) { this->verbose_ = verbose; this->name = name; this->argLists.push_back(args); @@ -26,16 +27,16 @@ void GlobalFunction::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void GlobalFunction::matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const { +void GlobalFunction::matlab_proxy(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const { // cluster overloads with same namespace // create new GlobalFunction structures around namespaces - same namespaces and names are overloads // map of namespace to global function typedef map GlobalFunctionMap; GlobalFunctionMap grouped_functions; - for (size_t i=0; i& functionNames) const { +void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const { // create the folder for the namespace const StrVec& ns = namespaces.front(); @@ -68,20 +70,18 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons // open destination mfunctionFileName string mfunctionFileName = toolboxPath; - if(!ns.empty()) + if (!ns.empty()) mfunctionFileName += "/+" + wrap::qualifiedName("/+", ns); mfunctionFileName += "/" + name + ".m"; FileWriter mfunctionFile(mfunctionFileName, verbose_, "%"); // get the name of actual matlab object - const string - matlabQualName = qualifiedName(".", ns, name), - matlabUniqueName = qualifiedName("", ns, name), - cppName = qualifiedName("::", ns, name); + const string matlabQualName = qualifiedName(".", ns, name), matlabUniqueName = + qualifiedName("", ns, name), cppName = qualifiedName("::", ns, name); mfunctionFile.oss << "function varargout = " << name << "(varargin)\n"; - for(size_t overload = 0; overload < argLists.size(); ++overload) { + for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; size_t nrArgs = args.size(); @@ -91,14 +91,18 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons // Output proxy matlab code // check for number of arguments... - mfunctionFile.oss << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) mfunctionFile.oss << " && "; + mfunctionFile.oss << (overload == 0 ? "" : "else") + << "if length(varargin) == " << nrArgs; + if (nrArgs > 0) + mfunctionFile.oss << " && "; // ...and their types bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + + boost::lexical_cast(id); // call - wrapperFile.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start - wrapperFile.oss << "{\n"; + file.oss << "{\n"; - returnVal.wrapTypeUnwrap(wrapperFile); + returnVal.wrapTypeUnwrap(file); // check arguments // NOTE: for static functions, there is no object passed - wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "\",nargout,nargin," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << matlabUniqueName + << "\",nargout,nargin," << args.size() << ");\n"; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(wrapperFile,0); // We start at 0 because there is no self object + args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1!="void") - returnVal.wrap_result(cppName+"("+args.names()+")", wrapperFile, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result(cppName + "(" + args.names() + ")", file, + typeAttributes); else - wrapperFile.oss << cppName+"("+args.names()+");\n"; + file.oss << cppName + "(" + args.names() + ");\n"; // finish - wrapperFile.oss << "}\n"; + file.oss << "}\n"; // Add to function list functionNames.push_back(wrapFunctionName); } mfunctionFile.oss << "else\n"; - mfunctionFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << "');" << endl; + mfunctionFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "');" << endl; mfunctionFile.oss << "end" << endl; // Close file @@ -148,9 +158,5 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons /* ************************************************************************* */ - } // \namespace wrap - - - diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 2ecaf4998..6c8ad0c86 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -22,37 +22,38 @@ struct GlobalFunction { std::string name; // each overload, regardless of namespace - std::vector argLists; ///< arugments for each overload - std::vector returnVals; ///< returnVals for each overload - std::vector namespaces; ///< Stack of namespaces + std::vector argLists; ///< arugments for each overload + std::vector returnVals; ///< returnVals for each overload + std::vector namespaces; ///< Stack of namespaces // Constructor only used in Module - GlobalFunction(bool verbose = true) : verbose_(verbose) {} + GlobalFunction(bool verbose = true) : + verbose_(verbose) { + } // Used to reconstruct - GlobalFunction(const std::string& name_, bool verbose = true) - : verbose_(verbose), name(name_) {} + GlobalFunction(const std::string& name_, bool verbose = true) : + verbose_(verbose), name(name_) { + } // adds an overloaded version of this function void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack); + const ArgumentList& args, const ReturnValue& retVal, + const StrVec& ns_stack); // codegen function called from Module to build the cpp and matlab versions of the function - void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const; + void matlab_proxy(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const; private: // Creates a single global function - all in same namespace - void generateSingleFunction(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const; + void generateSingleFunction(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const; }; } // \namespace wrap - - - From 1129b384b2010c76c94fc5f04b00e46a9d864476 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:03:28 -0400 Subject: [PATCH 56/70] Header order --- wrap/tests/testWrap.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 8bf2c1412..2cf152a7d 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -15,16 +15,18 @@ * @author Frank Dellaert **/ +#include +#include + +#include + +#include +#include + #include #include #include #include -#include -#include -#include - -#include -#include using namespace std; using namespace boost::assign; From 925f23515d0103bbf01b8eef64703cb8f024abd1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:27:29 -0400 Subject: [PATCH 57/70] Better reporting of whitespace changes only --- wrap/utilities.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 870ba3101..47f7d10a6 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -88,9 +88,11 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) string actual_contents = file_contents(actual, skipheader); bool equal = actual_contents == expected_contents; if (!equal) { + cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n"; stringstream command; - command << "diff " << expected << " " << actual << endl; + command << "diff --ignore-all-space " << expected << " " << actual << endl; system(command.str().c_str()); + cerr << ">>>\n"; } return equal; } From 5e9632e7813328a00f4bac27c3aafedb4a3f8a47 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:28:39 -0400 Subject: [PATCH 58/70] Now using emit_conditional_call (changed indenting) --- wrap/GlobalFunction.cpp | 31 ++++++--------------------- wrap/tests/expected/aGlobalFunction.m | 10 ++++----- 2 files changed, 11 insertions(+), 30 deletions(-) diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 26d52018e..25e1dcedb 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -84,32 +84,13 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); const int id = functionNames.size(); // Output proxy matlab code - - // check for number of arguments... - mfunctionFile.oss << (overload == 0 ? "" : "else") - << "if length(varargin) == " << nrArgs; - if (nrArgs > 0) - mfunctionFile.oss << " && "; - // ...and their types - bool first = true; - for (size_t i = 0; i < nrArgs; i++) { - if (!first) - mfunctionFile.oss << " && "; - mfunctionFile.oss << "isa(varargin{" << i + 1 << "},'" - << args[i].matlabClass(".") << "')"; - first = false; - } - mfunctionFile.oss << "\n"; - - // output call to C++ wrapper - mfunctionFile.oss << " "; - returnVal.emit_matlab(mfunctionFile); - mfunctionFile.oss << wrapperName << "(" << id << ", varargin{:});\n"; + mfunctionFile.oss << " " << (overload == 0 ? "" : "else"); + argLists[overload].emit_conditional_call(mfunctionFile, + returnVals[overload], wrapperName, id, true); // true omits "this" // Output C++ wrapper code @@ -146,11 +127,11 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, functionNames.push_back(wrapFunctionName); } - mfunctionFile.oss << "else\n"; + mfunctionFile.oss << " else\n"; mfunctionFile.oss - << " error('Arguments do not match any overload of function " + << " error('Arguments do not match any overload of function " << matlabQualName << "');" << endl; - mfunctionFile.oss << "end" << endl; + mfunctionFile.oss << " end" << endl; // Close file mfunctionFile.emit(true); diff --git a/wrap/tests/expected/aGlobalFunction.m b/wrap/tests/expected/aGlobalFunction.m index 09ece0b83..94e9b4a67 100644 --- a/wrap/tests/expected/aGlobalFunction.m +++ b/wrap/tests/expected/aGlobalFunction.m @@ -1,6 +1,6 @@ function varargout = aGlobalFunction(varargin) -if length(varargin) == 0 - varargout{1} = geometry_wrapper(42, varargin{:}); -else - error('Arguments do not match any overload of function aGlobalFunction'); -end + if length(varargin) == 0 + varargout{1} = geometry_wrapper(42, varargin{:}); + else + error('Arguments do not match any overload of function aGlobalFunction'); + end From 399c5e555167df20ea8845a37dccaf27bcab07d2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:28:59 -0400 Subject: [PATCH 59/70] Added test for overloaded global functions --- wrap/tests/expected/geometry_wrapper.cpp | 19 +++++++++++++++++++ .../tests/expected/overloadedGlobalFunction.m | 8 ++++++++ wrap/tests/geometry.h | 4 ++++ wrap/tests/testWrap.cpp | 5 +++-- 4 files changed, 34 insertions(+), 2 deletions(-) create mode 100644 wrap/tests/expected/overloadedGlobalFunction.m diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index e00004d57..b34112718 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -502,6 +502,19 @@ void aGlobalFunction_42(int nargout, mxArray *out[], int nargin, const mxArray * checkArguments("aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(aGlobalFunction()); } +void overloadedGlobalFunction_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,1); + int a = unwrap< int >(in[0]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a)); +} +void overloadedGlobalFunction_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,2); + int a = unwrap< int >(in[0]); + double b = unwrap< double >(in[1]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a,b)); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -643,6 +656,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 42: aGlobalFunction_42(nargout, out, nargin-1, in+1); break; + case 43: + overloadedGlobalFunction_43(nargout, out, nargin-1, in+1); + break; + case 44: + overloadedGlobalFunction_44(nargout, out, nargin-1, in+1); + break; } } catch(const std::exception& e) { mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); diff --git a/wrap/tests/expected/overloadedGlobalFunction.m b/wrap/tests/expected/overloadedGlobalFunction.m new file mode 100644 index 000000000..5b086b15e --- /dev/null +++ b/wrap/tests/expected/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'numeric') + varargout{1} = geometry_wrapper(43, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') + varargout{1} = geometry_wrapper(44, varargin{:}); + else + error('Arguments do not match any overload of function overloadedGlobalFunction'); + end diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index bdced45ed..bc233763d 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -87,6 +87,10 @@ class Test { Vector aGlobalFunction(); +// An overloaded global function +Vector overloadedGlobalFunction(int a); +Vector overloadedGlobalFunction(int a, double b); + // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 2cf152a7d..290372c09 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -307,8 +307,8 @@ TEST( wrap, parse_geometry ) { } // evaluate global functions -// Vector aGlobalFunction(); - LONGS_EQUAL(1, module.global_functions.size()); + // Vector aGlobalFunction(); + LONGS_EQUAL(2, module.global_functions.size()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); @@ -447,6 +447,7 @@ TEST( wrap, matlab_code_geometry ) { EXPECT(files_equal(epath + "Point3.m" , apath + "Point3.m" )); EXPECT(files_equal(epath + "Test.m" , apath + "Test.m" )); EXPECT(files_equal(epath + "aGlobalFunction.m" , apath + "aGlobalFunction.m" )); + EXPECT(files_equal(epath + "overloadedGlobalFunction.m" , apath + "overloadedGlobalFunction.m" )); } /* ************************************************************************* */ From 5048946ae945a22dd6185107bc4cdc22ea359da7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:37:43 -0400 Subject: [PATCH 60/70] Make sure it works for namespaces as well --- .../+ns2/overloadedGlobalFunction.m | 8 +++++++ .../testNamespaces_wrapper.cpp | 21 +++++++++++++++++++ wrap/tests/testNamespaces.h | 4 ++++ wrap/tests/testWrap.cpp | 20 +++++++++++------- 4 files changed, 45 insertions(+), 8 deletions(-) create mode 100644 wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m diff --git a/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m b/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m new file mode 100644 index 000000000..84f3b8f47 --- /dev/null +++ b/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'ns1.ClassA') + varargout{1} = testNamespaces_wrapper(24, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'ns1.ClassA') && isa(varargin{2},'double') + varargout{1} = testNamespaces_wrapper(25, varargin{:}); + else + error('Arguments do not match any overload of function ns2.overloadedGlobalFunction'); + end diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp index 29739a2f6..6d22e1625 100644 --- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp +++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp @@ -342,6 +342,21 @@ void ns2aGlobalFunction_23(int nargout, mxArray *out[], int nargin, const mxArra checkArguments("ns2aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(ns2::aGlobalFunction()); } +void ns2overloadedGlobalFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedClassA; + checkArguments("ns2overloadedGlobalFunction",nargout,nargin,1); + ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); + out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a))),"ns1.ClassA", false); +} +void ns2overloadedGlobalFunction_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedClassA; + checkArguments("ns2overloadedGlobalFunction",nargout,nargin,2); + ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); + double b = unwrap< double >(in[1]); + out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a,b))),"ns1.ClassA", false); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -426,6 +441,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 23: ns2aGlobalFunction_23(nargout, out, nargin-1, in+1); break; + case 24: + ns2overloadedGlobalFunction_24(nargout, out, nargin-1, in+1); + break; + case 25: + ns2overloadedGlobalFunction_25(nargout, out, nargin-1, in+1); + break; } } catch(const std::exception& e) { mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h index 8e6ef51d6..a9b23cad1 100644 --- a/wrap/tests/testNamespaces.h +++ b/wrap/tests/testNamespaces.h @@ -47,6 +47,10 @@ class ClassC { // separate namespace global function, same name Vector aGlobalFunction(); +// An overloaded global function +ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a); +ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a, double b); + } //\namespace ns2 class ClassD { diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 290372c09..c6ce0903a 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -382,7 +382,7 @@ TEST( wrap, parse_namespaces ) { // evaluate global functions // Vector ns1::aGlobalFunction(); // Vector ns2::aGlobalFunction(); - LONGS_EQUAL(1, module.global_functions.size()); + LONGS_EQUAL(2, module.global_functions.size()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); @@ -417,13 +417,17 @@ TEST( wrap, matlab_code_namespaces ) { module.matlab_code("actual_namespaces", headerPath); - EXPECT(files_equal(exp_path + "ClassD.m" , act_path + "ClassD.m" )); - EXPECT(files_equal(exp_path + "+ns1/ClassA.m" , act_path + "+ns1/ClassA.m" )); - EXPECT(files_equal(exp_path + "+ns1/ClassB.m" , act_path + "+ns1/ClassB.m" )); - EXPECT(files_equal(exp_path + "+ns2/ClassA.m" , act_path + "+ns2/ClassA.m" )); - EXPECT(files_equal(exp_path + "+ns2/ClassC.m" , act_path + "+ns2/ClassC.m" )); - EXPECT(files_equal(exp_path + "+ns2/+ns3/ClassB.m" , act_path + "+ns2/+ns3/ClassB.m" )); - EXPECT(files_equal(exp_path + "testNamespaces_wrapper.cpp" , act_path + "testNamespaces_wrapper.cpp" )); + EXPECT(files_equal(exp_path + "ClassD.m", act_path + "ClassD.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassA.m", act_path + "+ns1/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassB.m", act_path + "+ns1/ClassB.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassA.m", act_path + "+ns2/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassC.m", act_path + "+ns2/ClassC.m" )); + EXPECT( + files_equal(exp_path + "+ns2/overloadedGlobalFunction.m", exp_path + "+ns2/overloadedGlobalFunction.m" )); + EXPECT( + files_equal(exp_path + "+ns2/+ns3/ClassB.m", act_path + "+ns2/+ns3/ClassB.m" )); + EXPECT( + files_equal(exp_path + "testNamespaces_wrapper.cpp", act_path + "testNamespaces_wrapper.cpp" )); } /* ************************************************************************* */ From 852e1e1f2f17419d78deda410d44b13bfc3f9ec3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 17:46:30 -0400 Subject: [PATCH 61/70] Drastic speedup of plotting --- matlab/+gtsam/plot2DTrajectory.m | 54 +++++++++++++------------------- 1 file changed, 21 insertions(+), 33 deletions(-) diff --git a/matlab/+gtsam/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m index c6b0f85aa..45e7fe467 100644 --- a/matlab/+gtsam/plot2DTrajectory.m +++ b/matlab/+gtsam/plot2DTrajectory.m @@ -8,53 +8,41 @@ function plot2DTrajectory(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'k*-'; + linespec = 'k*-'; end haveMarginals = exist('marginals', 'var'); -keys = KeyVector(values.keys); holdstate = ishold; hold on -% Plot poses and covariance matrices -lastIndex = []; -for i = 0:keys.size-1 +% Do something very efficient to draw trajectory +poses = utilities.extractPose2(values); +X = poses(:,1); +Y = poses(:,2); +theta = poses(:,3); +plot(X,Y,linespec); + +% Quiver can also be vectorized if no marginals asked +if ~haveMarginals + C = cos(theta); + S = sin(theta); + quiver(X,Y,C,S,0.1,linespec); +else + % plotPose2 does both quiver and covariance matrix + keys = KeyVector(values.keys); + for i = 0:keys.size-1 key = keys.at(i); x = values.at(key); if isa(x, 'gtsam.Pose2') - if ~isempty(lastIndex) - % Draw line from last pose then covariance ellipse on top of - % last pose. - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - gtsam.plotPose2(lastPose, 'g', P); - else - gtsam.plotPose2(lastPose, 'g', []); - end - - end - lastIndex = i; - end -end - -% Draw final covariance ellipse -if ~isempty(lastIndex) - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - gtsam.plotPose2(lastPose, 'g', P); - else - gtsam.plotPose2(lastPose, 'g', []); + P = marginals.marginalCovariance(key); + gtsam.plotPose2(x,linespec(1), P); end + end end if ~holdstate - hold off + hold off end end From c2e748b362e34ab07835a9cceb9358a53228d469 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 17:46:47 -0400 Subject: [PATCH 62/70] This example stopped working: now fixed --- matlab/gtsam_examples/Pose2SLAMExample_advanced.m | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/matlab/gtsam_examples/Pose2SLAMExample_advanced.m b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m index 343dee05b..64cda5afc 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_advanced.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m @@ -59,16 +59,16 @@ params.setAbsoluteErrorTol(1e-15); params.setRelativeErrorTol(1e-15); params.setVerbosity('ERROR'); params.setVerbosityDL('VERBOSE'); -params.setOrdering(graph.orderingCOLAMD(initialEstimate)); +params.setOrdering(graph.orderingCOLAMD()); optimizer = DoglegOptimizer(graph, initialEstimate, params); result = optimizer.optimizeSafely(); result.print('final result'); %% Get the corresponding dense matrix -ord = graph.orderingCOLAMD(result); -gfg = graph.linearize(result,ord); -denseAb = gfg.denseJacobian; +ord = graph.orderingCOLAMD(); +gfg = graph.linearize(result); +denseAb = gfg.augmentedJacobian; %% Get sparse matrix A and RHS b IJS = gfg.sparseJacobian_(); From 499f2f291855368e53bec198df7b9d8307f63cc5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 17:47:12 -0400 Subject: [PATCH 63/70] Small comments in examples (and timing of marginals) --- matlab/gtsam_examples/Pose2SLAMExample_graph.m | 2 ++ matlab/gtsam_examples/Pose3SLAMExample_graph.m | 2 ++ 2 files changed, 4 insertions(+) diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index b4957cce3..83ec949cc 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -36,7 +36,9 @@ toc hold on; plot2DTrajectory(result, 'b-*'); %% Plot Covariance Ellipses +tic marginals = Marginals(graph, result); +toc P={}; for i=1:result.size()-1 pose_i = result.at(i); diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m index 01df4fc33..39e48c204 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -12,6 +12,8 @@ import gtsam.* +%% PLEASE NOTE THAT PLOTTING TAKES A VERY LONG TIME HERE + %% Find data file N = 2500; % dataset = 'sphere_smallnoise.graph'; From 020d2e43f8f782b3ba20511e19383c45bf8ceec7 Mon Sep 17 00:00:00 2001 From: jing Date: Sun, 25 May 2014 20:09:49 -0400 Subject: [PATCH 64/70] change cmake option into correct way: use set CMAKE_CXX_FLAGS replace add_definition --- cmake/GtsamBuildTypes.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index ee2bbd42a..9a0b297ab 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -56,7 +56,7 @@ endif() # Clang on Mac uses a template depth that is less than standard and is too small if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0") - add_definitions(-ftemplate-depth=1024) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") endif() endif() From f4cad73b73234b1851aaa0d49c27edf75948fee3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 26 May 2014 23:29:14 -0400 Subject: [PATCH 65/70] Fixed a problem with initialization (exposed by victoria_park.txt) and cleaned up a bit. Also fixed variances -> std deviation. --- gtsam/slam/dataset.cpp | 75 +++++++++++++++++++++++------------------- 1 file changed, 41 insertions(+), 34 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4a7166f38..772c0ec97 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -112,14 +112,14 @@ pair load2D( // Create a sampler with random number generator Sampler sampler(42u); - // load the factors + // Parse the pose constraints + int id1, id2; bool haveLandmark = false; while (is) { if(! (is >> tag)) break; if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { - int id1, id2; double x, y, yaw; double v1, v2, v3, v4, v5, v6; @@ -168,68 +168,75 @@ pair load2D( new BetweenFactor(id1, id2, l1Xl2, *model)); graph->push_back(factor); } + + // Parse measurements + double bearing, range, bearing_std, range_std; + + // A bearing-range measurement if (tag == "BR") { - int id1, id2; - double bearing, range, bearing_std, range_std; - is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; - - // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) - continue; - - noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); - - // Insert poses or points if they do not exist yet - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) { - Pose2 pose = initial->at(id1); - Point2 local(cos(bearing)*range,sin(bearing)*range); - Point2 global = pose.transform_from(local); - initial->insert(id2, global); - } } + + // A landmark measurement, TODO Frank says: don't know why is converted to bearing-range if (tag == "LANDMARK") { - int id1, id2; double lmx, lmy; double v1, v2, v3; is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3; // Convert x,y to bearing,range - double bearing = std::atan2(lmy, lmx); - double range = std::sqrt(lmx*lmx + lmy*lmy); + bearing = std::atan2(lmy, lmx); + range = std::sqrt(lmx*lmx + lmy*lmy); // In our experience, the x-y covariance on landmark sightings is not very good, so assume - // that it describes the uncertainty at a range of 10m, and convert that to bearing/range - // uncertainty. - SharedDiagonal measurementNoise; + // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty. if(std::abs(v1 - v3) < 1e-4) { - double rangeVar = v1; - double bearingVar = v1 / 10.0; - measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar)); + bearing_std = sqrt(v1 / 10.0); + range_std = sqrt(v1); } else { + bearing_std = 1; + range_std = 1; if(!haveLandmark) { cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n" "non-uniform covariance on LANDMARK measurements in this file." << endl; haveLandmark = true; } } + } + + // Do some common stuff for bearing-range measurements + if (tag == "LANDMARK" || tag == "BR") { + + // optional filter + if (maxID && id1 >= maxID) + continue; + + // Create noise model + noiseModel::Diagonal::shared_ptr measurementNoise = + noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); // Add to graph - *graph += BearingRangeFactor(id1, L(id2), bearing, range, measurementNoise); + *graph += BearingRangeFactor(id1, L(id2), bearing, range, + measurementNoise); + + // Insert poses or points if they do not exist yet + if (!initial->exists(id1)) + initial->insert(id1, Pose2()); + if (!initial->exists(L(id2))) { + Pose2 pose = initial->at(id1); + Point2 local(cos(bearing)*range,sin(bearing)*range); + Point2 global = pose.transform_from(local); + initial->insert(L(id2), global); + } } is.ignore(LINESIZE, '\n'); } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } From 3b10f61e5c426bc1e57493de1701d7ccd4c51ab0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 27 May 2014 00:42:03 -0400 Subject: [PATCH 66/70] utilities.localToWorld --- gtsam.h | 2 ++ matlab.h | 32 ++++++++++++++++++++++++++++++++ matlab/+gtsam/Contents.m | 1 + 3 files changed, 35 insertions(+) diff --git a/gtsam.h b/gtsam.h index 99aec3f86..96a778acf 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2381,6 +2381,8 @@ namespace utilities { void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); } //\namespace utilities diff --git a/matlab.h b/matlab.h index e9ad519bd..c4891a615 100644 --- a/matlab.h +++ b/matlab.h @@ -28,6 +28,8 @@ #include #include +#include + #include namespace gtsam { @@ -218,6 +220,36 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph, return errors; } +/// Convert from local to world coordinates +Values localToWorld(const Values& local, const Pose2& base, + const FastVector user_keys = FastVector()) { + + Values world; + + // if no keys given, get all keys from local values + FastVector keys(user_keys); + if (keys.size()==0) + keys = FastVector(local.keys()); + + // Loop over all keys + BOOST_FOREACH(Key key, keys) { + try { + // if value is a Pose2, compose it with base pose + Pose2 pose = local.at(key); + world.insert(key, base.compose(pose)); + } catch (std::exception e1) { + try { + // if value is a Point2, transform it from base pose + Point2 point = local.at(key); + world.insert(key, base.transform_from(point)); + } catch (std::exception e2) { + // if not Pose2 or Point2, do nothing + } + } + } + return world; +} + } } diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 7a7491462..023c61dbe 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -189,3 +189,4 @@ % utilities.insertBackprojections - Insert a number of initial point values by backprojecting % utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key % utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph +% utilities.localToWorld - Convert from local to world coordinates From e730da95c4e67e6bd97b30e2b684e1bce5b53d0a Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 28 May 2014 09:17:31 -0400 Subject: [PATCH 67/70] Smarter noise models: Diagonal::Sigmas is now actually smart, and Gaussian::SqrtInformation now has a smart flag (default is true) --- gtsam/linear/NoiseModel.cpp | 63 ++++++++++++++++++++------- gtsam/linear/NoiseModel.h | 6 +-- gtsam/linear/tests/testNoiseModel.cpp | 18 ++++++++ 3 files changed, 69 insertions(+), 18 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 641b47640..4bce597a1 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -47,22 +47,50 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) { Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose(); } +/* ************************************************************************* */ +// check *above the diagonal* for non-zero entries +static boost::optional checkIfDiagonal(const Matrix M) { + size_t m = M.rows(), n = M.cols(); + // check all non-diagonal entries + bool full = false; + size_t i, j; + for (i = 0; i < m; i++) + if (!full) + for (j = i + 1; j < n; j++) + if (fabs(M(i, j)) > 1e-9) { + full = true; + break; + } + if (full) { + return boost::none; + } else { + Vector diagonal(n); + for (j = 0; j < n; j++) + diagonal(j) = M(j, j); + return diagonal; + } +} + +/* ************************************************************************* */ +Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { + size_t m = R.rows(), n = R.cols(); + if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square"); + boost::optional diagonal = boost::none; + if (smart) + diagonal = checkIfDiagonal(R); + if (diagonal) return Diagonal::Sigmas(reciprocal(*diagonal),true); + else return shared_ptr(new Gaussian(R.rows(),R)); +} /* ************************************************************************* */ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) { size_t m = covariance.rows(), n = covariance.cols(); if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square"); - if (smart) { - // check all non-diagonal entries - size_t i,j; - for (i = 0; i < m; i++) - for (j = 0; j < n; j++) - if (i != j && fabs(covariance(i, j)) > 1e-9) goto full; - Vector variances(n); - for (j = 0; j < n; j++) variances(j) = covariance(j,j); - return Diagonal::Variances(variances,true); - } - full: return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); + boost::optional variances = boost::none; + if (smart) + variances = checkIfDiagonal(covariance); + if (variances) return Diagonal::Variances(*variances,true); + else return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); } /* ************************************************************************* */ @@ -166,7 +194,7 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const // Diagonal /* ************************************************************************* */ Diagonal::Diagonal() : - Gaussian(1)//, sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1)) + Gaussian(1) // TODO: Frank asks: really sure about this? { } @@ -191,12 +219,17 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { /* ************************************************************************* */ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { if (smart) { + DenseIndex j, n = sigmas.size(); // look for zeros to make a constraint - for (size_t i=0; i< (size_t) sigmas.size(); ++i) - if (sigmas(i)<1e-8) + for (j=0; j< n; ++j) + if (sigmas(j)<1e-8) return Constrained::MixedSigmas(sigmas); + // check whether all the same entry + for (j = 1; j < n; j++) + if (sigmas(j) != sigmas(0)) goto full; + return Isotropic::Sigma(n, sigmas(0), true); } - return Diagonal::shared_ptr(new Diagonal(sigmas)); + full: return Diagonal::shared_ptr(new Diagonal(sigmas)); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index a87f426fa..e709ea543 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -159,10 +159,10 @@ namespace gtsam { /** * A Gaussian noise model created by specifying a square root information matrix. + * @param R The (upper-triangular) square root information matrix + * @param smart check if can be simplified to derived class */ - static shared_ptr SqrtInformation(const Matrix& R) { - return shared_ptr(new Gaussian(R.rows(),R)); - } + static shared_ptr SqrtInformation(const Matrix& R, bool smart = true); /** * A Gaussian noise model created by specifying a covariance matrix. diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 9d87a163b..d68caeabe 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -267,6 +267,24 @@ TEST(NoiseModel, QRNan ) EXPECT(assert_equal(expectedAb,Ab)); } +/* ************************************************************************* */ +TEST(NoiseModel, SmartSqrtInformation ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Create(3); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart); + EXPECT(assert_equal(*expected,*actual)); +} + +/* ************************************************************************* */ +TEST(NoiseModel, SmartSqrtInformation2 ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart); + EXPECT(assert_equal(*expected,*actual)); +} + /* ************************************************************************* */ TEST(NoiseModel, SmartCovariance ) { From b7fc6762af9667a9654ffe7c190304f3d3506dec Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 28 May 2014 10:27:01 -0400 Subject: [PATCH 68/70] Changes every time ? --- examples/Data/dubrovnik-3-7-pre-rewritten.txt | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/examples/Data/dubrovnik-3-7-pre-rewritten.txt b/examples/Data/dubrovnik-3-7-pre-rewritten.txt index 12c9f4db4..7dea43c9e 100644 --- a/examples/Data/dubrovnik-3-7-pre-rewritten.txt +++ b/examples/Data/dubrovnik-3-7-pre-rewritten.txt @@ -20,9 +20,9 @@ 1 6 543.18011474609375 294.80999755859375 2 6 -58.419979095458984375 110.8300018310546875 -0.29656188120312942935 + 0.29656188120312942935 -0.035318354384285870207 -0.31252101755032046793 + 0.31252101755032046793 0.47230274932665988752 -0.3572340863744113415 -2.0517704282499575896 @@ -30,21 +30,21 @@ -7.5572756941255647689e-08 3.2377570134516087119e-14 -0.28532097381985194184 + 0.28532097381985194184 -0.27699838370789808817 0.048601169984112867206 --1.2598695987143850861 + -1.2598695987143850861 -0.049063798188844320869 --1.9586867140445654023 + -1.9586867140445654023 1432.137451171875 -7.3171918302250560373e-08 3.1759419042137054801e-14 0.057491325683772541433 -0.34853090049579965592 -0.47985129303736057116 -8.1963904289063389541 -6.5146840788718787252 + 0.34853090049579965592 + 0.47985129303736057116 + 8.1963904289063389541 + 6.5146840788718787252 -3.8392804395897406344 1572.047119140625 -1.5962623223231275915e-08 From 8dba25f532000e274815b3995f89f483a46eecce Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 28 May 2014 10:36:26 -0400 Subject: [PATCH 69/70] Rationalized some cholesky-related code as I was looking at it. --- .cproject | 1848 +++++++++++++++---------------- gtsam/base/Matrix.cpp | 47 +- gtsam/base/tests/testMatrix.cpp | 36 +- 3 files changed, 952 insertions(+), 979 deletions(-) diff --git a/.cproject b/.cproject index da776f062..af6b32cd2 100644 --- a/.cproject +++ b/.cproject @@ -568,7 +568,6 @@ make - tests/testBayesTree.run true false @@ -576,7 +575,6 @@ make - testBinaryBayesNet.run true false @@ -624,7 +622,6 @@ make - testSymbolicBayesNet.run true false @@ -632,7 +629,6 @@ make - tests/testSymbolicFactor.run true false @@ -640,7 +636,6 @@ make - testSymbolicFactorGraph.run true false @@ -656,20 +651,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -686,134 +672,6 @@ true true - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - make -j5 @@ -854,46 +712,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - make -j2 @@ -910,134 +728,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - make -j5 @@ -1078,6 +768,30 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -1126,46 +840,6 @@ true true - - make - -j5 - testWrap.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -1334,294 +1008,6 @@ true true - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j2 - vSFMexample.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testGraph.run - true - false - true - - - make - - testJunctionTree.run - true - false - true - - - make - - testSymbolicBayesNetB.run - true - false - true - - - make - -j5 - testGaussianISAM.run - true - true - true - - - make - -j5 - testDoglegOptimizer.run - true - true - true - - - make - -j5 - testNonlinearFactorGraph.run - true - true - true - - - make - -j5 - testIterative.run - true - true - true - - - make - -j5 - testSubgraphSolver.run - true - true - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - - - make - -j5 - testSummarization.run - true - true - true - make -j5 @@ -1728,7 +1114,6 @@ make - testErrors.run true false @@ -1862,14 +1247,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1966,54 +1343,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -2104,6 +1433,7 @@ make + testSimulated2DOriented.run true false @@ -2143,6 +1473,7 @@ make + testSimulated2D.run true false @@ -2150,6 +1481,7 @@ make + testSimulated3D.run true false @@ -2179,22 +1511,6 @@ true true - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - make -j5 @@ -2235,181 +1551,6 @@ false true - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make -j2 @@ -2627,7 +1768,6 @@ cpack - -G DEB true false @@ -2635,7 +1775,6 @@ cpack - -G RPM true false @@ -2643,7 +1782,6 @@ cpack - -G TGZ true false @@ -2651,7 +1789,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2825,6 +1962,898 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j6 -j8 + testAntiFactor.run + true + true + true + + + make + -j6 -j8 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + testJunctionTree.run + true + false + true + + + make + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + + + make + -j5 + testNonlinearFactorGraph.run + true + true + true + + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + + + make + -j5 + testSummarization.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + true + make -j2 @@ -2921,45 +2950,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - true - false - true - make -j5 diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index d9ab7d71c..f51197cf2 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -591,41 +591,17 @@ Matrix3 skewSymmetric(double wx, double wy, double wz) } /* ************************************************************************* */ -/** Numerical Recipes in C wrappers - * create Numerical Recipes in C structure - * pointers are subtracted by one to provide base 1 access - */ -/* ************************************************************************* */ -// FIXME: assumes row major, rather than column major -//double** createNRC(Matrix& A) { -// const size_t m=A.rows(); -// double** a = new double* [m]; -// for(size_t i = 0; i < m; i++) -// a[i] = &A(i,0)-1; -// return a; -//} - -/* ****************************************** - * - * Modified from Justin's codebase - * - * Idea came from other public domain code. Takes a S.P.D. matrix - * and computes the LL^t decomposition. returns L, which is lower - * triangular. Note this is the opposite convention from Matlab, - * which calculates Q'Q where Q is upper triangular. - * - * ******************************************/ Matrix LLt(const Matrix& A) { - Matrix L = zeros(A.rows(), A.rows()); - Eigen::LLT llt; - llt.compute(A); + Eigen::LLT llt(A); return llt.matrixL(); } +/* ************************************************************************* */ Matrix RtR(const Matrix &A) { - return LLt(A).transpose(); + Eigen::LLT llt(A); + return llt.matrixU(); } /* @@ -633,13 +609,10 @@ Matrix RtR(const Matrix &A) */ Matrix cholesky_inverse(const Matrix &A) { - // FIXME: replace with real algorithm - return A.inverse(); - -// Matrix L = LLt(A); -// Matrix inv(eye(A.rows())); -// inplace_solve (L, inv, BNU::lower_tag ()); -// return BNU::prod(trans(inv), inv); + Eigen::LLT llt(A); + Matrix inv = eye(A.rows()); + llt.matrixU().solveInPlace(inv); + return inv*inv.transpose(); } /* ************************************************************************* */ @@ -648,9 +621,9 @@ Matrix cholesky_inverse(const Matrix &A) // inv(B) * inv(B)' == A // inv(B' * B) == A Matrix inverse_square_root(const Matrix& A) { - Matrix R = RtR(A); + Eigen::LLT llt(A); Matrix inv = eye(A.rows()); - R.triangularView().solveInPlace(inv); + llt.matrixU().solveInPlace(inv); return inv.transpose(); } diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 9d84e5ab7..4e857b143 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1022,22 +1022,32 @@ TEST( matrix, inverse_square_root ) /* *********************************************************************** */ // M was generated as the covariance of a set of random numbers. L that // we are checking against was generated via chol(M)' on octave +namespace cholesky { +Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, + 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, + 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, + 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, + -0.0021113, 0.0036415, 0.0909464); + +Matrix expected = (Matrix(5, 5) << + 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, + -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, + 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, + 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, + 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); +} TEST( matrix, LLt ) { - Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, - 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, - 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, - 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, - -0.0021113, 0.0036415, 0.0909464); + EQUALITY(cholesky::expected, LLt(cholesky::M)); +} +TEST( matrix, RtR ) +{ + EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M)); +} - Matrix expected = (Matrix(5, 5) << - 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, - -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, - 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, - 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, - 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); - - EQUALITY(expected, LLt(M)); +TEST( matrix, cholesky_inverse ) +{ + EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } /* ************************************************************************* */ From 26996059437fcdc831111700d58b27e490460e0a Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 28 May 2014 11:31:17 -0400 Subject: [PATCH 70/70] Fix return type mismatch --- gtsam_unstable/slam/SmartFactorBase.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index 265df5ecd..974a37706 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -136,12 +136,12 @@ public: } /** return the measurements */ - const Vector& measured() const { + const std::vector& measured() const { return measured_; } /** return the noise model */ - const SharedNoiseModel& noise() const { + const std::vector& noise() const { return noise_; }