Lab 7 Deliverable 3

[votmA]

I have been stuck in lab 7 at the deliverable 3.
Probably, I am doing something wrong. But I could not understand where. Any advice would be appreciated. The error is that the optimizer quits after the 1st iteration stating that the error increased.

Here is the code for 3a:

  // Error function.
  // @param p 3D pose.
  // @param H optional Jacobian matrix.
  gtsam::Vector evaluateError(
      const gtsam::Pose3& p,
      boost::optional<gtsam::Matrix&> H = boost::none) const {

    // 3a. Complete definition of factor
    // TODO: 
    // Return error vector and jacobian if requested (aka H !=
    // boost::none).
    //
    // Insert code below:
    // Extract the rotation component from the pose.
    Vector3 translation = p.translation();
    Rot3 rotation = p.rotation();

    if (H) {
      Matrix H_tmp = Matrix::Zero(3, 6);
      H_tmp.block<3, 3>(0, 0) = rotation.matrix(); 
      H_tmp.block<3, 3>(0, 3) = I_3x3;
      (*H) = H_tmp;
    }

    return (Vector(3) << translation.x() - m_.x(), translation.y() - m_.y(), translation.z() - m_.z()).finished();
    // End 3a. 
  }

Here is the code for 3b:

  if (use_mocap) {
    // 3b. Add motion capture measurements (mocap_measurements)
    // TODO: create the 3D MoCap measurement noise model (a diagonal noise
    // model should be sufficient). Think of how many dimensions it should have.
    // You are given mocap_std_dev (defined above)
    //
    //

    //  TODO: add the MoCap factors
    // Note that there is no prior factor needed at first pose, since MoCap
    // provides the global positions (and rotations given more than 1 MoCap
    // measurement).
    //
    // Insert code below:
    noiseModel::Diagonal::shared_ptr mocapNoise =
    noiseModel::Diagonal::Sigmas(Vector3(mocap_std_dev, mocap_std_dev, mocap_std_dev));

    for (const auto& mocap_measurement : mocap_measurements) {
        // Extract information from the mocap_measurement
        int pose_key = mocap_measurement.first;
        Point3 mocap_position = mocap_measurement.second;

        // Add a MoCap factor to the graph
        graph.add(boost::make_shared<MoCapPosition3Factor>(pose_key, mocap_position, mocapNoise));
    }
    // End 3b. 
  }