NULL-jacobians are handled correctly in generated autodiff code

The autodiff code generator is extended in the following ways:
  - Evaluate the complete jacobian into a temporary array
  - Copy the temporary jacobian to the result if != NULL

Change-Id: If5ed0bff3b31e93a2d7d5dc718862cb648dba474

include/ceres/codegen/autodiff.h

diff --git a/include/ceres/codegen/autodiff.h b/include/ceres/codegen/autodiff.h
index 3160bb3..0a2a65d 100644
--- a/include/ceres/codegen/autodiff.h
+++ b/include/ceres/codegen/autodiff.h
@@ -131,7 +131,7 @@
   //    jacobians[0][1] = v_352;
   //    ...
   for (int i = 0, total_param_id = 0; i < kNumParameterBlocks;
-       ++i, total_param_id += ParameterDims::GetDim(i)) {
+       total_param_id += ParameterDims::GetDim(i), ++i) {
     for (int r = 0; r < kNumResiduals; ++r) {
       for (int j = 0; j < ParameterDims::GetDim(i); ++j) {
         internal::MakeOutput(
@@ -191,20 +191,65 @@
   // EvaluateResidualAndJacobian. This combined function is compatible to
   // CostFunction::Evaluate. Therefore the generated code can be directly used
   // in SizedCostFunctions.
-
   output.emplace_back("bool Evaluate(double const* const* parameters,");
   output.emplace_back("              double* residuals,");
-  output.emplace_back("              double** jacobians)");
-  output.emplace_back("{");
-  output.emplace_back("   if (residuals && jacobians) {");
-  output.emplace_back("     EvaluateResidualAndJacobian(");
-  output.emplace_back("         parameters,");
-  output.emplace_back("         residuals,");
-  output.emplace_back("         jacobians);");
+  output.emplace_back("              double** jacobians) {");
+  output.emplace_back("   if (jacobians) {");
+
+  // Create a tmp array of all jacobians and use it for evaluation.
+  // The generated code for a <2,3,1,2> cost functor is:
+  //   double jacobians_data[6];
+  //   double* jacobians_ptrs[] = {
+  //       jacobians_data + 0,
+  //       jacobians_data + 6,
+  //       jacobians_data + 8,
+  //   };
+  output.emplace_back("     double jacobians_data[" +
+                      std::to_string(kNumParameters * kNumResiduals) + "];");
+  output.emplace_back("     double* jacobians_ptrs[] = {");
+  for (int i = 0, total_param_id = 0; i < kNumParameterBlocks;
+       total_param_id += ParameterDims::GetDim(i), ++i) {
+    output.emplace_back("       jacobians_data + " +
+                        std::to_string(kNumResiduals * total_param_id) + ",");
+  }
+  output.emplace_back("     };");
+
+  // Evaluate into the tmp array.
+  output.emplace_back(
+      "     EvaluateResidualAndJacobian(parameters, residuals, "
+      "jacobians_ptrs);");
+
+  // Copy the computed jacobians into the output array. Add an if-statement to
+  // test for null-jacobians. The generated code for a <2,3,1,2> cost functor
+  // is:
+  //    if (jacobians[0]) {
+  //      for (int i = 0; i < 6; ++i) {
+  //        jacobians[0][i] = jacobians_tmp[0][i];
+  //      }
+  //    }
+  //    if (jacobians[1]) {
+  //      for (int i = 0; i < 2; ++i) {
+  //        jacobians[1][i] = jacobians_tmp[1][i];
+  //      }
+  //    }
+  //    if (jacobians[2]) {
+  //      for (int i = 0; i < 4; ++i) {
+  //        jacobians[2][i] = jacobians_tmp[2][i];
+  //      }
+  //    }
+  for (int i = 0; i < kNumParameterBlocks; ++i) {
+    output.emplace_back("     if (jacobians[" + std::to_string(i) + "]) {");
+    output.emplace_back(
+        "       for (int i = 0; i < " +
+        std::to_string(ParameterDims::GetDim(i) * kNumResiduals) + "; ++i) {");
+    output.emplace_back("         jacobians[" + std::to_string(i) +
+                        "][i] = jacobians_ptrs[" + std::to_string(i) + "][i];");
+    output.emplace_back("       }");
+    output.emplace_back("     }");
+  }
+  output.emplace_back("     return true;");
   output.emplace_back("   }");
-  output.emplace_back("   else if (residuals) {");
-  output.emplace_back("     EvaluateResidual(parameters,residuals);");
-  output.emplace_back("   }");
+  output.emplace_back("   EvaluateResidual(parameters, residuals);");
   output.emplace_back("   return true;");
   output.emplace_back("}");