SUPERFAST linearize, by pre-allocating the JacobianFactor and writing into it.

Merging local branch 'feature/BAD_WriteableJacobianFactor' into feature/BAD

.cproject

@@ -600,7 +600,6 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -608,7 +607,6 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -656,7 +654,6 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -664,7 +661,6 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -672,7 +668,6 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -688,7 +683,6 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1120,7 +1114,6 @@
 			</target>
 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testErrors.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1350,46 +1343,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testBTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSF.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFMap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testFixedVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1472,6 +1425,7 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1511,6 +1465,7 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1518,6 +1473,7 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1531,6 +1487,46 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSF.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFMap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testFixedVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -1788,7 +1784,6 @@
 			</target>
 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G DEB</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1796,7 +1791,6 @@
 			</target>
 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G RPM</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1804,7 +1798,6 @@
 			</target>
 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G TGZ</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1812,7 +1805,6 @@
 			</target>
 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2441,6 +2433,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testVerticalBlockMatrix.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testVerticalBlockMatrix.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -2579,7 +2579,6 @@
 			</target>
 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2587,7 +2586,6 @@
 			</target>
 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2595,7 +2593,6 @@
 			</target>
 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNetB.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -3115,6 +3112,7 @@
 			</target>
 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testGaussianISAM2</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>

gtsam/base/VerticalBlockMatrix.h

@@ -65,9 +65,10 @@ namespace gtsam {
 
     /** Construct from a container of the sizes of each vertical block. */
     template<typename CONTAINER>
-    VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) :
+    VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height,
-      rowStart_(0), rowEnd_(height), blockStart_(0)
+        bool appendOneDimension = false) :
-    {
+        variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
+        rowStart_(0), rowEnd_(height), blockStart_(0) {
       fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
       matrix_.resize(height, variableColOffsets_.back());
       assertInvariants();
@@ -75,21 +76,23 @@ namespace gtsam {
 
     /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
     template<typename CONTAINER, typename DERIVED>
-    VerticalBlockMatrix(const CONTAINER& dimensions, const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
+    VerticalBlockMatrix(const CONTAINER& dimensions,
-      matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0)
+        const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
-    {
+        matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
+        rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) {
       fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
       if (variableColOffsets_.back() != matrix_.cols())
-        throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
+        throw std::invalid_argument(
+            "Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
       assertInvariants();
     }
 
-    /**
+    /** Construct from iterator over the sizes of each vertical block. */
-     * Construct from iterator over the sizes of each vertical block. */
     template<typename ITERATOR>
-    VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) :
+    VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
-      rowStart_(0), rowEnd_(height), blockStart_(0)
+        DenseIndex height, bool appendOneDimension = false) :
-    {
+        variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)),
+        rowStart_(0), rowEnd_(height), blockStart_(0) {
       fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
       matrix_.resize(height, variableColOffsets_.back());
       assertInvariants();
@@ -203,18 +206,12 @@ namespace gtsam {
 
     template<typename ITERATOR>
     void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) {
-      variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
       variableColOffsets_[0] = 0;
       DenseIndex j=0;
-      for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
+      for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j)
         variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
-        ++ j;
-      }
       if(appendOneDimension)
-      {
         variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
-        ++ j;
-      }
     }
 
     friend class SymmetricBlockMatrix;

gtsam/base/tests/testVerticalBlockMatrix.cpp

@@ -24,9 +24,20 @@ using namespace std;
 using namespace gtsam;
 using boost::assign::list_of;
 
+list<size_t> L = list_of(3)(2)(1);
+vector<size_t> dimensions(L.begin(),L.end());
+
 //*****************************************************************************
-TEST(VerticalBlockMatrix, constructor) {
+TEST(VerticalBlockMatrix, Constructor1) {
-  VerticalBlockMatrix actual(list_of(3)(2)(1),
+  VerticalBlockMatrix actual(dimensions,6);
+  EXPECT_LONGS_EQUAL(6,actual.rows());
+  EXPECT_LONGS_EQUAL(6,actual.cols());
+  EXPECT_LONGS_EQUAL(3,actual.nBlocks());
+}
+
+//*****************************************************************************
+TEST(VerticalBlockMatrix, Constructor2) {
+  VerticalBlockMatrix actual(dimensions,
       (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, //
       2, 8, 9, 10, 11, 12, //
       3, 9, 15, 16, 17, 18, //
@@ -38,6 +49,14 @@ TEST(VerticalBlockMatrix, constructor) {
   EXPECT_LONGS_EQUAL(3,actual.nBlocks());
 }
 
+//*****************************************************************************
+TEST(VerticalBlockMatrix, Constructor3) {
+  VerticalBlockMatrix actual(dimensions.begin(),dimensions.end(),6);
+  EXPECT_LONGS_EQUAL(6,actual.rows());
+  EXPECT_LONGS_EQUAL(6,actual.cols());
+  EXPECT_LONGS_EQUAL(3,actual.nBlocks());
+}
+
 //*****************************************************************************
 int main() {
   TestResult tr;

gtsam/linear/JacobianFactor.h

@@ -82,20 +82,22 @@ namespace gtsam {
   class GTSAM_EXPORT JacobianFactor : public GaussianFactor
   {
   public:
+
     typedef JacobianFactor This; ///< Typedef to this class
     typedef GaussianFactor Base; ///< Typedef to base class
     typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
 
-  protected:
-    VerticalBlockMatrix Ab_;      // the block view of the full matrix
-    noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
-
-  public:
     typedef VerticalBlockMatrix::Block ABlock;
     typedef VerticalBlockMatrix::constBlock constABlock;
     typedef ABlock::ColXpr BVector;
     typedef constABlock::ConstColXpr constBVector;
 
+  protected:
+
+    VerticalBlockMatrix Ab_;      // the block view of the full matrix
+    noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
+
+  public:
 
     /** Convert from other GaussianFactor */
     explicit JacobianFactor(const GaussianFactor& gf);
@@ -328,6 +330,21 @@ namespace gtsam {
     
   private:
 
+    /** Unsafe Constructor that creates an uninitialized Jacobian of right size
+     *  @param keys in some order
+     *  @param diemnsions of the variables in same order
+     *  @param m output dimension
+     *  @param model noise model (default NULL)
+     */
+    template<class KEYS, class DIMENSIONS>
+    JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
+        const SharedDiagonal& model = SharedDiagonal()) :
+        Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
+    }
+
+    // be very selective on who can access these private methods:
+    template<typename T> friend class ExpressionFactor;
+
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -107,23 +107,23 @@ public:
 
   virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
 
-    // This method has been heavily optimized for maximum performance.
+    // Check whether noise model is constrained or not
-    // We allocate a VerticalBlockMatrix on the stack first, and then create
+    noiseModel::Constrained::shared_ptr constrained = //
-    // a JacobianMap view onto it, which is then passed
+        boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
-    // to [expression_.value] to allow it to write directly into Ab_.
 
-    // Another malloc saved by creating a Matrix on the stack
+    // Create a writeable JacobianFactor in advance
-    double memory[Dim * augmentedCols_];
+    boost::shared_ptr<JacobianFactor> factor(
-    Eigen::Map<Eigen::Matrix<double, Dim, Eigen::Dynamic> > //
+        constrained ? new JacobianFactor(keys_, dimensions_, Dim,
-    matrix(memory, Dim, augmentedCols_);
+            constrained->unit()) :
-    matrix.setZero(); // zero out
+            new JacobianFactor(keys_, dimensions_, Dim));
-
-    // Construct block matrix, is of right size but un-initialized
-    VerticalBlockMatrix Ab(dimensions_, matrix, true);
 
     // Wrap keys and VerticalBlockMatrix into structure passed to expression_
+    VerticalBlockMatrix& Ab = factor->matrixObject();
     JacobianMap map(keys_, Ab);
 
+    // Zero out Jacobian so we can simply add to it
+    Ab.matrix().setZero();
+
     // Evaluate error to get Jacobians and RHS vector b
     T value = expression_.value(x, map); // <<< Reverse AD happens here !
     Ab(size()).col(0) = -measurement_.localCoordinates(value);
@@ -131,15 +131,7 @@ public:
     // Whiten the corresponding system now
     // TODO ! this->noiseModel_->WhitenSystem(Ab);
 
-    // TODO pass unwhitened + noise model to Gaussian factor
+    return factor;
-    // For now, only linearized constrained factors have noise model at linear level!!!
-    noiseModel::Constrained::shared_ptr constrained = //
-        boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
-    if (constrained) {
-      return boost::make_shared<JacobianFactor>(this->keys(), Ab,
-          constrained->unit());
-    } else
-      return boost::make_shared<JacobianFactor>(this->keys(), Ab);
   }
 };
 // ExpressionFactor

gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp

@@ -28,6 +28,9 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <boost/assign/list_of.hpp>
+using boost::assign::list_of;
+
 using namespace std;
 using namespace gtsam;
 

gtsam_unstable/timing/timeCameraExpression.cpp

@@ -27,7 +27,7 @@
 using namespace std;
 using namespace gtsam;
 
-#define time timeMultiThreaded
+#define time timeSingleThreaded
 
 boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());