text:
#include <ros/ros.h>
#include <geometry_msgs/WrenchStamped.h>
#include <nav_msgs/Odometry.h>
#include <iostream>
#include <string>
#include <fstream>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <boost/function.hpp>
#include <boost/bind.hpp>

using namespace std;
using namespace message_filters;

class Recorder
{
public:
	Recorder(ros::NodeHandle& nh);
	void write_oneline(const vector<double>& f_t_data, ofstream& outFile);
	void callback_quads(const nav_msgs::OdometryConstPtr& odom0,
							   const geometry_msgs::WrenchStamped::ConstPtr& data_ft0,
							   const nav_msgs::OdometryConstPtr& odom1,
							   const geometry_msgs::WrenchStamped::ConstPtr& data_ft1,
							   const nav_msgs::OdometryConstPtr& odom2,
							   const geometry_msgs::WrenchStamped::ConstPtr& data_ft2,
							   const nav_msgs::OdometryConstPtr& odom3,
							   const geometry_msgs::WrenchStamped::ConstPtr& data_ft3);

	void writeDataFromMsg(const nav_msgs::OdometryConstPtr& odom,
						  const geometry_msgs::WrenchStamped::ConstPtr& data_ft,
						  vector<double>& v_data_combine,
						  ofstream& outFile);

private:
	vector<string> namespaces;
	int num_data;
	ofstream outFile_0;
	ofstream outFile_1;
	ofstream outFile_2;
	ofstream outFile_3;

	string filename_0;
	string filename_1;
	string filename_2;
	string filename_3;

	std::vector<double> v_data_combine_0;
	std::vector<double> v_data_combine_1;
	std::vector<double> v_data_combine_2;
	std::vector<double> v_data_combine_3;

};

Recorder::Recorder(ros::NodeHandle& nh):
	num_data(9),
	v_data_combine_0(num_data, 0.0),
	v_data_combine_1(num_data, 0.0),
	v_data_combine_2(num_data, 0.0),
	v_data_combine_3(num_data, 0.0)
{
	time_t t = time(0);
    char tmp[64];
    strftime( tmp, sizeof(tmp), "%Y_%m_%d_%X",localtime(&t) );

    char c_ft_data0[100];
    sprintf(c_ft_data0, "hummingbird_0_%s.csv", tmp);
    cout<<"----- file force torque data: "<< c_ft_data0<<endl;
    filename_0 = c_ft_data0;

    char c_ft_data1[100];
    sprintf(c_ft_data1,"hummingbird_1_%s.csv",tmp);
    cout<<"----- file force torque data: "<< c_ft_data1<<endl;
    filename_1 = c_ft_data1;

    char c_ft_data2[100];
    sprintf(c_ft_data2,"hummingbird_2_%s.csv", tmp);
    cout<<"----- file force torque data: "<< c_ft_data2<<endl;
    filename_2 = c_ft_data2;

    char c_ft_data3[100];
    sprintf(c_ft_data3,"hummingbird_3_%s.csv", tmp);
    cout<<"----- file force torque data: "<< c_ft_data3<<endl;
    filename_3 = c_ft_data3;

    outFile_0.open(filename_0);
    outFile_1.open(filename_1);
    outFile_2.open(filename_2);
    outFile_3.open(filename_3);

  	message_filters::Subscriber<nav_msgs::Odometry> Odom_sub_0(nh, "/hummingbird_0/ground_truth/odometry", 2);
 	message_filters::Subscriber<geometry_msgs::WrenchStamped> ft_sub_0(nh, "/hummingbird_0/f_t", 2);

 	message_filters::Subscriber<nav_msgs::Odometry> Odom_sub_1(nh, "/hummingbird_1/ground_truth/odometry", 2);
 	message_filters::Subscriber<geometry_msgs::WrenchStamped> ft_sub_1(nh, "/hummingbird_1/f_t", 2);

 	message_filters::Subscriber<nav_msgs::Odometry> Odom_sub_2(nh, "/hummingbird_2/ground_truth/odometry", 2);
 	message_filters::Subscriber<geometry_msgs::WrenchStamped> ft_sub_2(nh, "/hummingbird_2/f_t", 2);

 	message_filters::Subscriber<nav_msgs::Odometry> Odom_sub_3(nh, "/hummingbird_3/ground_truth/odometry", 2);
 	message_filters::Subscriber<geometry_msgs::WrenchStamped> ft_sub_3(nh, "/hummingbird_3/f_t", 2);

 	typedef sync_policies::ApproximateTime<
 										   nav_msgs::Odometry, geometry_msgs::WrenchStamped,
 										   nav_msgs::Odometry, geometry_msgs::WrenchStamped,
 										   nav_msgs::Odometry, geometry_msgs::WrenchStamped,
 										   nav_msgs::Odometry, geometry_msgs::WrenchStamped
 										   > MySyncPolicy;

 	Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), Odom_sub_0, ft_sub_0, Odom_sub_1, ft_sub_1, Odom_sub_2, ft_sub_2, Odom_sub_3, ft_sub_3);
 	sync.registerCallback(boost::bind(&Recorder::callback_quads, this, _1, _2, _3, _4, _5, _6, _7, _8));

 	cout << "debug here before spin" << endl;
 	ros::spin();

}

void Recorder::callback_quads(const nav_msgs::OdometryConstPtr& odom0,
							   const geometry_msgs::WrenchStamped::ConstPtr& data_ft0,
							   const nav_msgs::OdometryConstPtr& odom1,
							   const geometry_msgs::WrenchStamped::ConstPtr& data_ft1,
							   const nav_msgs::OdometryConstPtr& odom2,
							   const geometry_msgs::WrenchStamped::ConstPtr& data_ft2,
							   const nav_msgs::OdometryConstPtr& odom3,
							   const geometry_msgs::WrenchStamped::ConstPtr& data_ft3)
{
	writeDataFromMsg(odom0, data_ft0, v_data_combine_0, outFile_0);

	writeDataFromMsg(odom1, data_ft1, v_data_combine_1, outFile_1);

	writeDataFromMsg(odom2, data_ft2, v_data_combine_2, outFile_2);

	writeDataFromMsg(odom3, data_ft3, v_data_combine_3, outFile_3);
}

void Recorder::writeDataFromMsg(const nav_msgs::OdometryConstPtr& odom,
							    const geometry_msgs::WrenchStamped::ConstPtr& data_ft,
								vector<double>& v_data_combine,
								ofstream& outFile)
{
	double temp_odom_ft_data[num_data] =
					{
						odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z,
						data_ft->wrench.force.x, data_ft->wrench.force.y, data_ft->wrench.force.z,
						data_ft->wrench.torque.x, data_ft->wrench.torque.y, data_ft->wrench.torque.z
					};

	if (!v_data_combine.empty())
	{
		v_data_combine.clear();
	}
	v_data_combine.insert(v_data_combine.begin(), temp_odom_ft_data, temp_odom_ft_data + num_data);
	// cout << v_data_combine[0] << endl << v_data_combine[1];

	write_oneline(v_data_combine, outFile);
}

void Recorder::write_oneline(const vector<double>& f_t_data, ofstream& outFile)
{
	// outFile_0.open(filename_0, ios::out);
	for(auto i_vec : f_t_data){
		char c[50];
		sprintf(c, "%f", i_vec);
		outFile << c << ",";
	}
	outFile << endl;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "force_recorder");
	ros::NodeHandle nh;
	Recorder recorder(nh);
	return 0;
}


text:
#pragma once

#include <QDebug>
#include <QElapsedTimer>
#include <boost/current_function.hpp>

#define BENCH(x)     \
    QElapsedTimer x; \
    x.start();

#define MARK(x)                                    \
    qDebug() << BOOST_CURRENT_FUNCTION << __LINE__ \
             << static_cast<float>(x.nsecsElapsed()) / 100000.0 << "ms";

ext: hpp


text:
//
// $Id$
//
//
// Original author: Darren Kessner <darren@proteowizard.org>
//
// Copyright 2008 Spielberg Family Center for Applied Proteomics
//   Cedars-Sinai Medical Center, Los Angeles, California  90048
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#include "RAMPAdapter.hpp"
#include "MSDataFile.hpp"
#include "examples.hpp"
#include "pwiz/utility/misc/unit.hpp"
#include "pwiz/utility/misc/Filesystem.hpp"
#include "pwiz/utility/misc/Std.hpp"
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/device/file_descriptor.hpp>
#include <boost/iostreams/copy.hpp>

using namespace pwiz::cv;
using namespace pwiz::msdata;
using namespace pwiz::util;

ostream* os_ = 0;

/*
RAMP will need to instantiate an msdata::RAMPAdapter object, and call it to fill
in the appropriate RAMP structures:

    using namespace pwiz;
using namespace pwiz::msdata;

    RAMPAdapter adapter("something.mzML");

    unsigned int scanIndex = rampAdapter.index(19); // get index for scan 19

    ScanHeaderStruct temp;
    adapter.getScanHeader(scanIndex, temp);

    vector<double> buffer;
    adapter.getScanPeaks(scanIndex, buffer);

    RunHeaderStruct temp2;
    adapter.getRunHeader(temp2);

    InstrumentHeaderStruct temp3;
    adapter.getInstrumentHeader(temp3);

Note that the MSData library can throw exceptions, so the RAMP code will have to be able
to handle these gracefully.  Another option is to have these RAMPAdapter functions catch
any exceptions and return an error code if something goes wrong.
*/

string writeTempFile()
{
    const string& filename = "temp.RAMPAdapterTest.tiny.mzML";
    MSData tiny;
    examples::initializeTiny(tiny);
    MSDataFile::write(tiny, filename);
    return filename;
}

ostream& operator<<(ostream& os, const ScanHeaderStruct& header)
{
   os << "seqNum: " << header.seqNum << endl;
   os << "acquisitionNum: " << header.acquisitionNum << endl;
   os << "msLevel: " << header.msLevel << endl;
   os << "activationMethod: " << header.activationMethod  << endl;
   os << "peaksCount: " << header.peaksCount << endl;
   os << "totIonCurrent: " << header.totIonCurrent << endl;
   os << "retentionTime: " << header.retentionTime << endl;
   os << "basePeakMZ: " << header.basePeakMZ << endl;
   os << "basePeakIntensity: " << header.basePeakIntensity << endl;
   os << "collisionEnergy: " << header.collisionEnergy << endl;
   os << "ionisationEnergy: " << header.ionisationEnergy << endl;
   os << "lowMZ: " << header.lowMZ << endl;
   os << "highMZ: " << header.highMZ << endl;
   os << "precursorScanNum: " << header.precursorScanNum << endl;
   os << "precursorMZ: " << header.precursorMZ << endl;
   os << "precursorCharge: " << header.precursorCharge << endl;
   os << "precursorIntensity: " << header.precursorIntensity << endl;
   os << "scanType: " << header.scanType << endl;
   os << "mergedScan: " << header.mergedScan << endl;
   os << "mergedResultScanNum: " << header.mergedResultScanNum << endl;
   os << "mergedResultStartScanNum: " << header.mergedResultStartScanNum << endl;
   os << "mergedResultEndScanNum: " << header.mergedResultEndScanNum << endl;
   os << "filePosition: " << header.filePosition << endl;
   return os;
}

ostream& operator<<(ostream& os, const RunHeaderStruct& header)
{
    os << "scanCount: " << header.scanCount << endl;
    os << "lowMZ: " << header.lowMZ << endl;
    os << "highMZ: " << header.highMZ << endl;
    os << "startMZ: " << header.startMZ << endl;
    os << "endMZ: " << header.endMZ << endl;
    os << "dStartTime: " << header.dStartTime << endl;
    os << "dEndTime: " << header.dEndTime << endl;
    return os;
}

ostream& operator<<(ostream& os, const InstrumentStruct& instrument)
{
    os << "manufacturer: " << instrument.manufacturer << endl;
    os << "model: " << instrument.model << endl;
    os << "ionisation: " << instrument.ionisation << endl;
    os << "analyzer: " << instrument.analyzer << endl;
    os << "detector: " << instrument.detector << endl;
    return os;
}

void test(const string& filename)
{
    RAMPAdapter adapter(filename);

    size_t scanCount = adapter.scanCount();
    if (os_) *os_ << "scanCount: " << scanCount << "\n\n";
    unit_assert(scanCount == 5);

    unit_assert(adapter.index(19) == 0);
    unit_assert(adapter.index(20) == 1);
    unit_assert(adapter.index(21) == 2);

    // first scan (scan number == 19)

    ScanHeaderStruct header1;
    adapter.getScanHeader(0, header1);
    if (os_) *os_ << header1;
    unit_assert(header1.seqNum == 1);
    unit_assert(header1.acquisitionNum == 19);
    unit_assert(header1.msLevel == 1);
    unit_assert(header1.peaksCount == 15);
    const double epsilon = 1e-8;
    unit_assert_equal(header1.totIonCurrent, 1.66755e7, epsilon);
    unit_assert_equal(header1.retentionTime, 353.43, epsilon);
    unit_assert_equal(header1.basePeakMZ, 445.347, epsilon);
    unit_assert_equal(header1.basePeakIntensity, 120053, epsilon);
    unit_assert_equal(header1.collisionEnergy, 0., epsilon);
    unit_assert_equal(header1.lowMZ, 400.39, epsilon);
    unit_assert_equal(header1.highMZ, 1795.56, epsilon);
    unit_assert(header1.precursorScanNum == 0);
    unit_assert(header1.scanType == string("Full"));
    unit_assert(header1.activationMethod == string(""));

    vector<double> peaks;
    adapter.getScanPeaks(0, peaks);
    unit_assert(peaks.size() == 30);
    if (os_)
    {
        const MZIntensityPair* begin = reinterpret_cast<const MZIntensityPair*>(&peaks[0]);
        copy(begin, begin+15, ostream_iterator<MZIntensityPair>(*os_, "\n"));
        *os_ << endl;
    }

    // second scan (scan number == 20)

    ScanHeaderStruct header2;
    adapter.getScanHeader(1, header2);
    if (os_) *os_ << header2;
    unit_assert(header2.seqNum == 2);
    unit_assert(header2.acquisitionNum == 20);
    unit_assert(header2.msLevel == 2);
    unit_assert(header2.activationMethod == string("collision-induced dissociation"));
    unit_assert(header2.peaksCount == 10);
    unit_assert_equal(header2.totIonCurrent, 1.66755e7, epsilon);
    unit_assert_equal(header2.retentionTime, 359.43, epsilon);
    unit_assert_equal(header2.basePeakMZ, 456.347, epsilon);
    unit_assert_equal(header2.basePeakIntensity, 23433, epsilon);
    unit_assert_equal(header2.collisionEnergy, 35, epsilon);
    unit_assert_equal(header2.lowMZ, 320.39, epsilon);
    unit_assert_equal(header2.highMZ, 1003.56, epsilon);
    unit_assert(header2.precursorScanNum == 19);
    unit_assert_equal(header2.precursorMZ, 445.34, epsilon);
    unit_assert(header2.precursorCharge == 2);
    unit_assert_equal(header2.precursorIntensity, 120053, epsilon);
    unit_assert(header2.scanType == string("Full"));

    adapter.getScanPeaks(1, peaks);
    unit_assert(peaks.size() == 20);
    if (os_)
    {
        const MZIntensityPair* begin = reinterpret_cast<const MZIntensityPair*>(&peaks[0]);
        copy(begin, begin+10, ostream_iterator<MZIntensityPair>(*os_, "\n"));
        *os_ << endl;
    }

    // last scan

    ScanHeaderStruct header5;
    adapter.getScanHeader(4, header5);
    if (os_) *os_ << header5;

    // RunHeader

    RunHeaderStruct runHeader;
    adapter.getRunHeader(runHeader);
    unit_assert(runHeader.scanCount == 5);
    unit_assert(runHeader.lowMZ == 0);
    unit_assert(runHeader.highMZ == 0);
    unit_assert(runHeader.startMZ == 0);
    unit_assert(runHeader.endMZ == 0);
    unit_assert_equal(runHeader.dStartTime, header1.retentionTime, 1e-6);
    unit_assert_equal(runHeader.dEndTime, header5.retentionTime, 1e-6);

    if (os_)
        *os_ << "RunHeader:\n" << runHeader << endl;

    // Instrument
    InstrumentStruct instrument;
    adapter.getInstrument(instrument);
    if (os_)
        *os_ << "Instrument:\n" << instrument << endl;

    unit_assert(!strcmp(instrument.manufacturer, "Thermo Finnigan"));
    unit_assert(!strcmp(instrument.model, "LCQ Deca"));
    unit_assert(!strcmp(instrument.ionisation, "nanoelectrospray"));
    unit_assert(!strcmp(instrument.analyzer, "quadrupole ion trap"));
    unit_assert(!strcmp(instrument.detector, "electron multiplier"));
}

static void test_mzML_1_0() {

    std::string srcparent(__FILE__);
    size_t pos = srcparent.find((bfs::path("pwiz") / "data").string());
    srcparent.resize(pos);

	std::string example_data_dir = srcparent + "example_data/";
	RAMPAdapter adapter_1_0(example_data_dir + "tiny.pwiz.1.0.mzML");
    const char *testfiles[2] = {"tiny.pwiz.1.1.mzML","tiny.pwiz.mzXML"};
    for (int tf=2;tf--;) { // test mzML 1.0 vs mzML 1.1 and mzXML
	    RAMPAdapter adapter_1_1(example_data_dir + testfiles[tf]);

        // tiny example has 4 spectra, the last of which is non-default source -- test scans 1,2,3 only
	    unit_assert(adapter_1_0.scanCount() == adapter_1_1.scanCount());
	    for (int scan=4;scan--;) {
		    ScanHeaderStruct header1_0, header1_1;
		    adapter_1_0.getScanHeader(scan, header1_0);
		    adapter_1_1.getScanHeader(scan, header1_1);
		    unit_assert(header1_0.seqNum == header1_1.seqNum );
            if (scan < 3) {
		        unit_assert(header1_0.acquisitionNum == header1_1.acquisitionNum );
            }
		    unit_assert(header1_0.msLevel == header1_1.msLevel );
		    unit_assert(header1_0.peaksCount == header1_1.peaksCount );
		    const double epsilon = 1e-6;
		    unit_assert_equal(header1_0.totIonCurrent, header1_1.totIonCurrent, epsilon);
		    unit_assert_equal(header1_0.retentionTime, header1_1.retentionTime, epsilon);
		    unit_assert_equal(header1_0.basePeakMZ, header1_1.basePeakMZ, epsilon);
		    unit_assert_equal(header1_0.basePeakIntensity, header1_1.basePeakIntensity, epsilon);
		    unit_assert_equal(header1_0.collisionEnergy, header1_1.collisionEnergy, epsilon);
		    unit_assert_equal(header1_0.lowMZ, header1_1.lowMZ, epsilon);
		    unit_assert_equal(header1_0.highMZ, header1_1.highMZ, epsilon);
		    unit_assert(header1_0.precursorScanNum == header1_1.precursorScanNum );
	    }
    }
}

int main(int argc, char* argv[])
{
    TEST_PROLOG(argc, argv)

    try
    {
        if (argc>1 && !strcmp(argv[1],"-v")) os_ = &cout;
        string filename = writeTempFile();
        test(filename);

		// now try it with a gzipped file
        string gzfilename = filename + ".gz";
		bio::filtering_istream tinyGZ(bio::gzip_compressor() | bio::file_descriptor_source(filename));
        bio::copy(tinyGZ, bio::file_descriptor_sink(gzfilename, ios::out|ios::binary));
        test(gzfilename);

        boost::filesystem::remove(filename);
        boost::filesystem::remove(gzfilename);
		// and make sure we're still good with older files
		test_mzML_1_0();

    }
    catch (exception& e)
    {
        TEST_FAILED(e.what())
    }
    catch (...)
    {
        TEST_FAILED("Caught unknown exception.")
    }

    TEST_EPILOG
}

ext: cpp

max_stars_repo_name: austinkeller/pwiz

max_issues_repo_name: austinkeller/pwiz

max_forks_repo_name: austinkeller/pwiz


text:
// Copyright 2013 Cloudera, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "kudu/tablet/tablet_bootstrap.h"

#include <boost/foreach.hpp>
#include <gflags/gflags.h>
#include <map>
#include <string>
#include <utility>
#include <vector>

#include "kudu/common/partial_row.h"
#include "kudu/common/row_operations.h"
#include "kudu/common/wire_protocol.h"
#include "kudu/consensus/consensus_meta.h"
#include "kudu/consensus/log.h"
#include "kudu/consensus/log_anchor_registry.h"
#include "kudu/consensus/log_reader.h"
#include "kudu/consensus/log_util.h"
#include "kudu/consensus/opid_util.h"
#include "kudu/fs/fs_manager.h"
#include "kudu/gutil/ref_counted.h"
#include "kudu/gutil/stl_util.h"
#include "kudu/gutil/strings/substitute.h"
#include "kudu/gutil/strings/strcat.h"
#include "kudu/gutil/strings/util.h"
#include "kudu/gutil/walltime.h"
#include "kudu/server/clock.h"
#include "kudu/server/hybrid_clock.h"
#include "kudu/server/metadata.h"
#include "kudu/tablet/lock_manager.h"
#include "kudu/tablet/row_op.h"
#include "kudu/tablet/tablet.h"
#include "kudu/tablet/tablet_peer.h"
#include "kudu/tablet/transactions/alter_schema_transaction.h"
#include "kudu/tablet/transactions/write_transaction.h"
#include "kudu/util/debug/trace_event.h"
#include "kudu/util/fault_injection.h"
#include "kudu/util/flag_tags.h"
#include "kudu/util/path_util.h"
#include "kudu/util/locks.h"
#include "kudu/util/logging.h"
#include "kudu/util/stopwatch.h"

DEFINE_bool(skip_remove_old_recovery_dir, false,
            "Skip removing WAL recovery dir after startup. (useful for debugging)");
TAG_FLAG(skip_remove_old_recovery_dir, hidden);

DEFINE_double(fault_crash_during_log_replay, 0.0,
              "Fraction of the time when the tablet will crash immediately "
              "after processing a log entry during log replay. "
              "(For testing only!)");
TAG_FLAG(fault_crash_during_log_replay, unsafe);

DECLARE_int32(max_clock_sync_error_usec);

namespace kudu {
namespace tablet {

using boost::shared_lock;
using consensus::ChangeConfigRecordPB;
using consensus::CommitMsg;
using consensus::ConsensusBootstrapInfo;
using consensus::ConsensusMetadata;
using consensus::ConsensusRound;
using consensus::MinimumOpId;
using consensus::OperationType;
using consensus::OperationType_Name;
using consensus::OpId;
using consensus::OpIdEquals;
using consensus::OpIdEqualsFunctor;
using consensus::OpIdHashFunctor;
using consensus::OpIdToString;
using consensus::RaftConfigPB;
using consensus::ReplicateMsg;
using consensus::ALTER_SCHEMA_OP;
using consensus::CHANGE_CONFIG_OP;
using consensus::NO_OP;
using consensus::WRITE_OP;
using log::Log;
using log::LogEntryPB;
using log::LogOptions;
using log::LogReader;
using log::LogAnchorRegistry;
using log::ReadableLogSegment;
using server::Clock;
using tserver::AlterSchemaRequestPB;
using tserver::WriteRequestPB;
using std::tr1::shared_ptr;
using strings::Substitute;

struct ReplayState;

// Information from the tablet metadata which indicates which data was
// flushed prior to this restart.
//
// We take a snapshot of this information at the beginning of the bootstrap
// process so that we can allow compactions and flushes to run during bootstrap
// without confusing our tracking of flushed stores.
class FlushedStoresSnapshot {
 public:
  FlushedStoresSnapshot() {}
  Status InitFrom(const TabletMetadata& meta);

  bool WasStoreAlreadyFlushed(const MemStoreTargetPB& target) const;

 private:
  int64_t last_durable_mrs_id_;
  std::tr1::unordered_map<int64_t, int64_t> flushed_dms_by_drs_id_;

  DISALLOW_COPY_AND_ASSIGN(FlushedStoresSnapshot);
};

// Bootstraps an existing tablet by opening the metadata from disk, and rebuilding soft
// state by playing log segments. A bootstrapped tablet can then be added to an existing
// consensus configuration as a LEARNER, which will bring its state up to date with the
// rest of the consensus configuration, or it can start serving the data itself, after it
// has been appointed LEADER of that particular consensus configuration.
//
// NOTE: this does not handle pulling data from other replicas in the cluster. That
// is handled by the 'RemoteBootstrap' classes, which copy blocks and metadata locally
// before invoking this local bootstrap functionality.
//
// TODO Because the table that is being rebuilt is never flushed/compacted, consensus
// is only set on the tablet after bootstrap, when we get to flushes/compactions though
// we need to set it before replay or we won't be able to re-rebuild.
class TabletBootstrap {
 public:
  TabletBootstrap(const scoped_refptr<TabletMetadata>& meta,
                  const scoped_refptr<Clock>& clock,
                  const shared_ptr<MemTracker>& mem_tracker,
                  MetricRegistry* metric_registry,
                  TabletStatusListener* listener,
                  const scoped_refptr<LogAnchorRegistry>& log_anchor_registry);

  // Plays the log segments, rebuilding the portion of the Tablet's soft
  // state that is present in the log (additional soft state may be present
  // in other replicas).
  // A successful call will yield the rebuilt tablet and the rebuilt log.
  Status Bootstrap(std::tr1::shared_ptr<Tablet>* rebuilt_tablet,
                   scoped_refptr<Log>* rebuilt_log,
                   ConsensusBootstrapInfo* results);

 private:

  // Opens the tablet.
  // Sets '*has_blocks' to true if there was any data on disk for this tablet.
  Status OpenTablet(bool* has_blocks);

  // Checks if a previous log recovery directory exists. If so, it deletes any
  // files in the log dir and sets 'needs_recovery' to true, meaning that the
  // previous recovery attempt should be retried from the recovery dir.
  //
  // Otherwise, if there is a log directory with log files in it, renames that
  // log dir to the log recovery dir and creates a new, empty log dir so that
  // log replay can proceed. 'needs_recovery' is also returned as true in this
  // case.
  //
  // If no log segments are found, 'needs_recovery' is set to false.
  Status PrepareRecoveryDir(bool* needs_recovery);

  // Opens the latest log segments for the Tablet that will allow to rebuild
  // the tablet's soft state. If there are existing log segments in the tablet's
  // log directly they are moved to a "log-recovery" directory which is deleted
  // when the replay process is completed (as they have been duplicated in the
  // current log directory).
  //
  // If a "log-recovery" directory is already present, we will continue to replay
  // from the "log-recovery" directory. Tablet metadata is updated once replay
  // has finished from the "log-recovery" directory.
  Status OpenLogReaderInRecoveryDir();

  // Opens a new log in the tablet's log directory.
  // The directory is expected to be clean.
  Status OpenNewLog();

  // Finishes bootstrap, setting 'rebuilt_log' and 'rebuilt_tablet'.
  Status FinishBootstrap(const std::string& message,
                         scoped_refptr<log::Log>* rebuilt_log,
                         shared_ptr<Tablet>* rebuilt_tablet);

  // Plays the log segments into the tablet being built.
  // The process of playing the segments generates a new log that can be continued
  // later on when then tablet is rebuilt and starts accepting writes from clients.
  Status PlaySegments(ConsensusBootstrapInfo* results);

  // Append the given commit message to the log.
  // Does not support writing a TxResult.
  Status AppendCommitMsg(const CommitMsg& commit_msg);

  Status PlayWriteRequest(ReplicateMsg* replicate_msg,
                          const CommitMsg& commit_msg);

  Status PlayAlterSchemaRequest(ReplicateMsg* replicate_msg,
                                const CommitMsg& commit_msg);

  Status PlayChangeConfigRequest(ReplicateMsg* replicate_msg,
                                 const CommitMsg& commit_msg);

  Status PlayNoOpRequest(ReplicateMsg* replicate_msg,
                         const CommitMsg& commit_msg);

  // Plays operations, skipping those that have already been flushed.
  Status PlayRowOperations(WriteTransactionState* tx_state,
                           const SchemaPB& schema_pb,
                           const RowOperationsPB& ops_pb,
                           const TxResultPB& result);

  // Pass through all of the decoded operations in tx_state. For
  // each op:
  // - if it was previously failed, mark as failed
  // - if it previously succeeded but was flushed, mark as skipped
  // - otherwise, re-apply to the tablet being bootstrapped.
  Status FilterAndApplyOperations(WriteTransactionState* tx_state,
                                  const TxResultPB& orig_result);

  // Filter a single insert operation, setting it to failed if
  // it was already flushed.
  Status FilterInsert(WriteTransactionState* tx_state,
                      RowOp* op,
                      const OperationResultPB& op_result);

  // Filter a single mutate operation, setting it to failed if
  // it was already flushed.
  Status FilterMutate(WriteTransactionState* tx_state,
                      RowOp* op,
                      const OperationResultPB& op_result);

  // Returns whether all the stores that are referred to in the commit
  // message are already flushed.
  bool AreAllStoresAlreadyFlushed(const CommitMsg& commit);

  // Returns whether there is any store that is referred to in the commit
  // message that is already flushed.
  bool AreAnyStoresAlreadyFlushed(const CommitMsg& commit);

  void DumpReplayStateToLog(const ReplayState& state);

  // Handlers for each type of message seen in the log during replay.
  Status HandleEntry(ReplayState* state, LogEntryPB* entry);
  Status HandleReplicateMessage(ReplayState* state, LogEntryPB* replicate_entry);
  Status HandleCommitMessage(ReplayState* state, LogEntryPB* commit_entry);
  Status ApplyCommitMessage(ReplayState* state, LogEntryPB* commit_entry);
  Status HandleEntryPair(LogEntryPB* replicate_entry, LogEntryPB* commit_entry);

  // Checks that an orphaned commit message is actually irrelevant, i.e that the
  // data stores it refers to are already flushed.
  Status CheckOrphanedCommitAlreadyFlushed(const CommitMsg& commit);

  // Decodes a Timestamp from the provided string and updates the clock
  // with it.
  Status UpdateClock(uint64_t timestamp);

  // Removes the recovery directory and all files contained therein.
  // Intended to be invoked after log replay successfully completes.
  Status RemoveRecoveryDir();

  // Return a log prefix string in the standard "T xxx P yyy" format.
  string LogPrefix() const;

  scoped_refptr<TabletMetadata> meta_;
  scoped_refptr<Clock> clock_;
  shared_ptr<MemTracker> mem_tracker_;
  MetricRegistry* metric_registry_;
  TabletStatusListener* listener_;
  gscoped_ptr<tablet::Tablet> tablet_;
  const scoped_refptr<log::LogAnchorRegistry> log_anchor_registry_;
  scoped_refptr<log::Log> log_;
  gscoped_ptr<log::LogReader> log_reader_;

  Arena arena_;

  gscoped_ptr<ConsensusMetadata> cmeta_;

  // Statistics on the replay of entries in the log.
  struct Stats {
    Stats()
      : ops_read(0),
        ops_overwritten(0),
        ops_committed(0),
        inserts_seen(0),
        inserts_ignored(0),
        mutations_seen(0),
        mutations_ignored(0),
        orphaned_commits(0) {
    }

    string ToString() const {
      return Substitute("ops{read=$0 overwritten=$1 applied=$2} "
                        "inserts{seen=$3 ignored=$4} "
                        "mutations{seen=$5 ignored=$6} "
                        "orphaned_commits=$7",
                        ops_read, ops_overwritten, ops_committed,
                        inserts_seen, inserts_ignored,
                        mutations_seen, mutations_ignored,
                        orphaned_commits);
    }

    // Number of REPLICATE messages read from the log
    int ops_read;
    // Number of REPLICATE messages which were overwritten by later entries.
    int ops_overwritten;
    // Number of REPLICATE messages for which a matching COMMIT was found.
    int ops_committed;

    // Number inserts/mutations seen and ignored.
    int inserts_seen, inserts_ignored;
    int mutations_seen, mutations_ignored;

    // Number of COMMIT messages for which a corresponding REPLICATE was not found.
    int orphaned_commits;
  };
  Stats stats_;

  // Snapshot of which stores were flushed prior to restart.
  FlushedStoresSnapshot flushed_stores_;

  DISALLOW_COPY_AND_ASSIGN(TabletBootstrap);
};

TabletStatusListener::TabletStatusListener(const scoped_refptr<TabletMetadata>& meta)
    : meta_(meta),
      last_status_("") {
}

const string TabletStatusListener::tablet_id() const {
  return meta_->tablet_id();
}

const string TabletStatusListener::table_name() const {
  return meta_->table_name();
}

const Partition& TabletStatusListener::partition() const {
  return meta_->partition();
}

const Schema& TabletStatusListener::schema() const {
  return meta_->schema();
}

TabletStatusListener::~TabletStatusListener() {
}

void TabletStatusListener::StatusMessage(const string& status) {
  LOG(INFO) << "T " << tablet_id() << " P " << meta_->fs_manager()->uuid() << ": "
            << status;
  boost::lock_guard<boost::shared_mutex> l(lock_);
  last_status_ = status;
}

Status BootstrapTablet(const scoped_refptr<TabletMetadata>& meta,
                       const scoped_refptr<Clock>& clock,
                       const shared_ptr<MemTracker>& mem_tracker,
                       MetricRegistry* metric_registry,
                       TabletStatusListener* listener,
                       std::tr1::shared_ptr<tablet::Tablet>* rebuilt_tablet,
                       scoped_refptr<log::Log>* rebuilt_log,
                       const scoped_refptr<log::LogAnchorRegistry>& log_anchor_registry,
                       ConsensusBootstrapInfo* consensus_info) {
  TRACE_EVENT1("tablet", "BootstrapTablet",
               "tablet_id", meta->tablet_id());
  TabletBootstrap bootstrap(meta, clock, mem_tracker,
                            metric_registry, listener, log_anchor_registry);
  RETURN_NOT_OK(bootstrap.Bootstrap(rebuilt_tablet, rebuilt_log, consensus_info));
  // This is necessary since OpenNewLog() initially disables sync.
  RETURN_NOT_OK((*rebuilt_log)->ReEnableSyncIfRequired());
  return Status::OK();
}

static string DebugInfo(const string& tablet_id,
                        int segment_seqno,
                        int entry_idx,
                        const string& segment_path,
                        const LogEntryPB& entry) {
  // Truncate the debug string to a reasonable length for logging.
  // Otherwise, glog will truncate for us and we may miss important
  // information which came after this long string.
  string debug_str = entry.ShortDebugString();
  if (debug_str.size() > 500) {
    debug_str.resize(500);
    debug_str.append("...");
  }
  return Substitute("Debug Info: Error playing entry $0 of segment $1 of tablet $2. "
                    "Segment path: $3. Entry: $4", entry_idx, segment_seqno, tablet_id,
                    segment_path, debug_str);
}

TabletBootstrap::TabletBootstrap(const scoped_refptr<TabletMetadata>& meta,
                                 const scoped_refptr<Clock>& clock,
                                 const shared_ptr<MemTracker>& mem_tracker,
                                 MetricRegistry* metric_registry,
                                 TabletStatusListener* listener,
                                 const scoped_refptr<LogAnchorRegistry>& log_anchor_registry)
    : meta_(meta),
      clock_(clock),
      mem_tracker_(mem_tracker),
      metric_registry_(metric_registry),
      listener_(listener),
      log_anchor_registry_(log_anchor_registry),
      arena_(256*1024, 4*1024*1024) {
}

Status TabletBootstrap::Bootstrap(shared_ptr<Tablet>* rebuilt_tablet,
                                  scoped_refptr<Log>* rebuilt_log,
                                  ConsensusBootstrapInfo* consensus_info) {
  string tablet_id = meta_->tablet_id();

  // Replay requires a valid Consensus metadata file to exist in order to
  // compare the committed consensus configuration seqno with the log entries and also to persist
  // committed but unpersisted changes.
  RETURN_NOT_OK_PREPEND(ConsensusMetadata::Load(meta_->fs_manager(), tablet_id,
                                                meta_->fs_manager()->uuid(), &cmeta_),
                        "Unable to load Consensus metadata");

  // Make sure we don't try to locally bootstrap a tablet that was in the middle
  // of a remote bootstrap. It's likely that not all files were copied over
  // successfully.
  TabletDataState tablet_data_state = meta_->tablet_data_state();
  if (tablet_data_state != TABLET_DATA_READY) {
    return Status::Corruption("Unable to locally bootstrap tablet " + tablet_id + ": " +
                              "TabletMetadata bootstrap state is " +
                              TabletDataState_Name(tablet_data_state));
  }

  meta_->PinFlush();

  listener_->StatusMessage("Bootstrap starting.");

  if (VLOG_IS_ON(1)) {
    TabletSuperBlockPB super_block;
    RETURN_NOT_OK(meta_->ToSuperBlock(&super_block));
    VLOG_WITH_PREFIX(1) << "Tablet Metadata: " << super_block.DebugString();
  }

  RETURN_NOT_OK(flushed_stores_.InitFrom(*meta_.get()));

  bool has_blocks;
  RETURN_NOT_OK(OpenTablet(&has_blocks));

  bool needs_recovery;
  RETURN_NOT_OK(PrepareRecoveryDir(&needs_recovery));
  if (needs_recovery) {
    RETURN_NOT_OK(OpenLogReaderInRecoveryDir());
  }

  // This is a new tablet, nothing left to do.
  if (!has_blocks && !needs_recovery) {
    LOG_WITH_PREFIX(INFO) << "No blocks or log segments found. Creating new log.";
    RETURN_NOT_OK_PREPEND(OpenNewLog(), "Failed to open new log");
    RETURN_NOT_OK(FinishBootstrap("No bootstrap required, opened a new log",
                                  rebuilt_log,
                                  rebuilt_tablet));
    consensus_info->last_id = MinimumOpId();
    consensus_info->last_committed_id = MinimumOpId();
    return Status::OK();
  }

  // If there were blocks, there must be segments to replay. This is required
  // by Raft, since we always need to know the term and index of the last
  // logged op in order to vote, know how to respond to AppendEntries(), etc.
  if (has_blocks && !needs_recovery) {
    return Status::IllegalState(Substitute("Tablet $0: Found rowsets but no log "
                                           "segments could be found.",
                                           tablet_id));
  }

  // Before playing any segments we set the safe and clean times to 'kMin' so that
  // the MvccManager will accept all transactions that we replay as uncommitted.
  tablet_->mvcc_manager()->OfflineAdjustSafeTime(Timestamp::kMin);
  RETURN_NOT_OK_PREPEND(PlaySegments(consensus_info), "Failed log replay. Reason");

  // Flush the consensus metadata once at the end to persist our changes, if any.
  cmeta_->Flush();

  RETURN_NOT_OK(RemoveRecoveryDir());
  RETURN_NOT_OK(FinishBootstrap("Bootstrap complete.", rebuilt_log, rebuilt_tablet));

  return Status::OK();
}

Status TabletBootstrap::FinishBootstrap(const std::string& message,
                                        scoped_refptr<log::Log>* rebuilt_log,
                                        shared_ptr<Tablet>* rebuilt_tablet) {
  // Add a callback to TabletMetadata that makes sure that each time we flush the metadata
  // we also wait for in-flights to finish and for their wal entry to be fsynced.
  // This might be a bit conservative in some situations but it will prevent us from
  // ever flushing the metadata referring to tablet data blocks containing data whose
  // commit entries are not durable, a pre-requisite for recovery.
  meta_->SetPreFlushCallback(
      Bind(&FlushInflightsToLogCallback::WaitForInflightsAndFlushLog,
           make_scoped_refptr(new FlushInflightsToLogCallback(tablet_.get(),
                                                              log_))));
  tablet_->MarkFinishedBootstrapping();
  RETURN_NOT_OK(tablet_->metadata()->UnPinFlush());
  listener_->StatusMessage(message);
  rebuilt_tablet->reset(tablet_.release());
  rebuilt_log->swap(log_);
  return Status::OK();
}

Status TabletBootstrap::OpenTablet(bool* has_blocks) {
  gscoped_ptr<Tablet> tablet(new Tablet(meta_,
                                        clock_,
                                        mem_tracker_,
                                        metric_registry_,
                                        log_anchor_registry_));
  // doing nothing for now except opening a tablet locally.
  LOG_TIMING_PREFIX(INFO, LogPrefix(), "opening tablet") {
    RETURN_NOT_OK(tablet->Open());
  }
  *has_blocks = tablet->num_rowsets() != 0;
  tablet_.reset(tablet.release());
  return Status::OK();
}

Status TabletBootstrap::PrepareRecoveryDir(bool* needs_recovery) {
  *needs_recovery = false;

  FsManager* fs_manager = tablet_->metadata()->fs_manager();
  string tablet_id = tablet_->metadata()->tablet_id();
  string log_dir = fs_manager->GetTabletWalDir(tablet_id);

  // If the recovery directory exists, then we crashed mid-recovery.
  // Throw away any logs from the previous recovery attempt and restart the log
  // replay process from the beginning using the same recovery dir as last time.
  string recovery_path = fs_manager->GetTabletWalRecoveryDir(tablet_id);
  if (fs_manager->Exists(recovery_path)) {
    LOG_WITH_PREFIX(INFO) << "Previous recovery directory found at " << recovery_path << ": "
                          << "Replaying log files from this location instead of " << log_dir;

    // Since we have a recovery directory, clear out the log_dir by recursively
    // deleting it and creating a new one so that we don't end up with remnants
    // of old WAL segments or indexes after replay.
    if (fs_manager->env()->FileExists(log_dir)) {
      LOG_WITH_PREFIX(INFO) << "Deleting old log files from previous recovery attempt in "
                            << log_dir;
      RETURN_NOT_OK_PREPEND(fs_manager->env()->DeleteRecursively(log_dir),
                            "Could not recursively delete old log dir " + log_dir);
    }

    RETURN_NOT_OK_PREPEND(fs_manager->CreateDirIfMissing(log_dir),
                          "Failed to create log directory " + log_dir);

    *needs_recovery = true;
    return Status::OK();
  }

  // If we made it here, there was no pre-existing recovery dir.
  // Now we look for log files in log_dir, and if we find any then we rename
  // the whole log_dir to a recovery dir and return needs_recovery = true.
  RETURN_NOT_OK_PREPEND(fs_manager->CreateDirIfMissing(log_dir),
                        "Failed to create log dir");

  vector<string> children;
  RETURN_NOT_OK_PREPEND(fs_manager->ListDir(log_dir, &children),
                        "Couldn't list log segments.");
  BOOST_FOREACH(const string& child, children) {
    if (!log::IsLogFileName(child)) {
      continue;
    }

    string source_path = JoinPathSegments(log_dir, child);
    string dest_path = JoinPathSegments(recovery_path, child);
    LOG_WITH_PREFIX(INFO) << "Will attempt to recover log segment " << source_path
                          << " to " << dest_path;
    *needs_recovery = true;
  }

  if (*needs_recovery) {
    // Atomically rename the log directory to the recovery directory
    // and then re-create the log directory.
    LOG_WITH_PREFIX(INFO) << "Moving log directory " << log_dir << " to recovery directory "
                          << recovery_path << " in preparation for log replay";
    RETURN_NOT_OK_PREPEND(fs_manager->env()->RenameFile(log_dir, recovery_path),
                          Substitute("Could not move log directory $0 to recovery dir $1",
                                     log_dir, recovery_path));
    RETURN_NOT_OK_PREPEND(fs_manager->env()->CreateDir(log_dir),
                          "Failed to recreate log directory " + log_dir);
  }
  return Status::OK();
}

Status TabletBootstrap::OpenLogReaderInRecoveryDir() {
  VLOG_WITH_PREFIX(1) << "Opening log reader in log recovery dir "
                      << meta_->fs_manager()->GetTabletWalRecoveryDir(tablet_->tablet_id());
  // Open the reader.
  RETURN_NOT_OK_PREPEND(LogReader::OpenFromRecoveryDir(tablet_->metadata()->fs_manager(),
                                                       tablet_->metadata()->tablet_id(),
                                                       tablet_->GetMetricEntity().get(),
                                                       &log_reader_),
                        "Could not open LogReader. Reason");
  return Status::OK();
}

Status TabletBootstrap::RemoveRecoveryDir() {
  FsManager* fs_manager = tablet_->metadata()->fs_manager();
  string recovery_path = fs_manager->GetTabletWalRecoveryDir(tablet_->metadata()->tablet_id());
  CHECK(fs_manager->Exists(recovery_path))
      << "Tablet WAL recovery dir " << recovery_path << " does not exist.";

  LOG_WITH_PREFIX(INFO) << "Preparing to delete log recovery files and directory " << recovery_path;

  string tmp_path = Substitute("$0-$1", recovery_path, GetCurrentTimeMicros());
  LOG_WITH_PREFIX(INFO) << "Renaming log recovery dir from "  << recovery_path
                        << " to " << tmp_path;
  RETURN_NOT_OK_PREPEND(fs_manager->env()->RenameFile(recovery_path, tmp_path),
                        Substitute("Could not rename old recovery dir from: $0 to: $1",
                                   recovery_path, tmp_path));

  if (FLAGS_skip_remove_old_recovery_dir) {
    LOG_WITH_PREFIX(INFO) << "--skip_remove_old_recovery_dir enabled. NOT deleting " << tmp_path;
    return Status::OK();
  }
  LOG_WITH_PREFIX(INFO) << "Deleting all files from renamed log recovery directory " << tmp_path;
  RETURN_NOT_OK_PREPEND(fs_manager->env()->DeleteRecursively(tmp_path),
                        "Could not remove renamed recovery dir " + tmp_path);
  LOG_WITH_PREFIX(INFO) << "Completed deletion of old log recovery files and directory "
                        << tmp_path;
  return Status::OK();
}

Status TabletBootstrap::OpenNewLog() {
  OpId init;
  init.set_term(0);
  init.set_index(0);
  RETURN_NOT_OK(Log::Open(LogOptions(),
                          tablet_->metadata()->fs_manager(),
                          tablet_->tablet_id(),
                          *tablet_->schema(),
                          tablet_->metadata()->schema_version(),
                          tablet_->GetMetricEntity(),
                          &log_));
  // Disable sync temporarily in order to speed up appends during the
  // bootstrap process.
  log_->DisableSync();
  return Status::OK();
}

typedef std::map<int64_t, LogEntryPB*> OpIndexToEntryMap;

// State kept during replay.
struct ReplayState {
  ReplayState()
    : prev_op_id(MinimumOpId()),
      committed_op_id(MinimumOpId()) {
  }

  ~ReplayState() {
    STLDeleteValues(&pending_replicates);
    STLDeleteValues(&pending_commits);
  }

  // Return true if 'b' is allowed to immediately follow 'a' in the log.
  static bool IsValidSequence(const OpId& a, const OpId& b) {
    if (a.term() == 0 && a.index() == 0) {
      // Not initialized - can start with any opid.
      return true;
    }

    // Within the same term, we should never skip entries.
    // We can, however go backwards (see KUDU-783 for an example)
    if (b.term() == a.term() &&
        b.index() > a.index() + 1) {
      return false;
    }

    return true;
  }

  // Return a Corruption status if 'id' seems to be out-of-sequence in the log.
  Status CheckSequentialReplicateId(const ReplicateMsg& msg) {
    DCHECK(msg.has_id());
    if (PREDICT_FALSE(!IsValidSequence(prev_op_id, msg.id()))) {
      string op_desc = Substitute("$0 REPLICATE (Type: $1)",
                                  OpIdToString(msg.id()),
                                  OperationType_Name(msg.op_type()));
      return Status::Corruption(
        Substitute("Unexpected opid following opid $0. Operation: $1",
                   OpIdToString(prev_op_id),
                   op_desc));
    }

    prev_op_id = msg.id();
    return Status::OK();
  }

  void UpdateCommittedOpId(const OpId& id) {
    if (id.index() > committed_op_id.index()) {
      committed_op_id = id;
    }
  }

  void AddEntriesToStrings(const OpIndexToEntryMap& entries, vector<string>* strings) const {
    BOOST_FOREACH(const OpIndexToEntryMap::value_type& map_entry, entries) {
      LogEntryPB* entry = DCHECK_NOTNULL(map_entry.second);
      strings->push_back(Substitute("   $0", entry->ShortDebugString()));
    }
  }

  void DumpReplayStateToStrings(vector<string>* strings)  const {
    strings->push_back(Substitute("ReplayState: Previous OpId: $0, Committed OpId: $1, "
        "Pending Replicates: $2, Pending Commits: $3", OpIdToString(prev_op_id),
        OpIdToString(committed_op_id), pending_replicates.size(), pending_commits.size()));
    if (!pending_replicates.empty()) {
      strings->push_back("Dumping REPLICATES: ");
      AddEntriesToStrings(pending_replicates, strings);
    }
    if (!pending_commits.empty()) {
      strings->push_back("Dumping COMMITS: ");
      AddEntriesToStrings(pending_commits, strings);
    }
  }

  // The last replicate message's ID.
  OpId prev_op_id;

  // The last operation known to be committed.
  // All other operations with lower IDs are also committed.
  OpId committed_op_id;

  // REPLICATE log entries whose corresponding COMMIT record has
  // not yet been seen. Keyed by index.
  OpIndexToEntryMap pending_replicates;

  // COMMIT log entries which couldn't be applied immediately.
  OpIndexToEntryMap pending_commits;
};

// Handle the given log entry. If OK is returned, then takes ownership of 'entry'.
// Otherwise, caller frees.
Status TabletBootstrap::HandleEntry(ReplayState* state, LogEntryPB* entry) {
  if (VLOG_IS_ON(1)) {
    VLOG_WITH_PREFIX(1) << "Handling entry: " << entry->ShortDebugString();
  }

  switch (entry->type()) {
    case log::REPLICATE:
      RETURN_NOT_OK(HandleReplicateMessage(state, entry));
      break;
    case log::COMMIT:
      // check the unpaired ops for the matching replicate msg, abort if not found
      RETURN_NOT_OK(HandleCommitMessage(state, entry));
      break;
    default:
      return Status::Corruption(Substitute("Unexpected log entry type: $0", entry->type()));
  }
  MAYBE_FAULT(FLAGS_fault_crash_during_log_replay);
  return Status::OK();
}

// Takes ownership of 'replicate_entry' on OK status.
Status TabletBootstrap::HandleReplicateMessage(ReplayState* state, LogEntryPB* replicate_entry) {
  stats_.ops_read++;

  const ReplicateMsg& replicate = replicate_entry->replicate();
  RETURN_NOT_OK(state->CheckSequentialReplicateId(replicate));
  DCHECK(replicate.has_timestamp());
  CHECK_OK(UpdateClock(replicate.timestamp()));

  // Append the replicate message to the log as is
  RETURN_NOT_OK(log_->Append(replicate_entry));

  int64_t index = replicate_entry->replicate().id().index();

  LogEntryPB** existing_entry_ptr = InsertOrReturnExisting(
      &state->pending_replicates, index, replicate_entry);

  // If there was a entry with the same index we're overwriting then we need to delete
  // that entry and all entries with higher indexes.
  if (existing_entry_ptr) {
    LogEntryPB* existing_entry = *existing_entry_ptr;

    OpIndexToEntryMap::iterator iter = state->pending_replicates.lower_bound(index);
    DCHECK(OpIdEquals((*iter).second->replicate().id(), existing_entry->replicate().id()));

    LogEntryPB* last_entry = (*state->pending_replicates.rbegin()).second;

    LOG_WITH_PREFIX(INFO) << "Overwriting operations starting at: "
                          << existing_entry->replicate().id()
                          << " up to: " << last_entry->replicate().id()
                          << " with operation: " << replicate_entry->replicate().id();

    while (iter != state->pending_replicates.end()) {
      delete (*iter).second;
      state->pending_replicates.erase(iter++);
      stats_.ops_overwritten++;
    }

    InsertOrDie(&state->pending_replicates, index, replicate_entry);
  }
  return Status::OK();
}

// Takes ownership of 'commit_entry' on OK status.
Status TabletBootstrap::HandleCommitMessage(ReplayState* state, LogEntryPB* commit_entry) {
  DCHECK(commit_entry->has_commit()) << "Not a commit message: " << commit_entry->DebugString();

  // Match up the COMMIT record with the original entry that it's applied to.
  const OpId& committed_op_id = commit_entry->commit().commited_op_id();
  state->UpdateCommittedOpId(committed_op_id);

  // If there are no pending replicates, or if this commit's index is lower than the
  // the first pending replicate on record this is likely an orphaned commit.
  if (state->pending_replicates.empty() ||
      (*state->pending_replicates.begin()).first > committed_op_id.index()) {
    VLOG_WITH_PREFIX(2) << "Found orphaned commit for " << committed_op_id;
    RETURN_NOT_OK(CheckOrphanedCommitAlreadyFlushed(commit_entry->commit()));
    stats_.orphaned_commits++;
    delete commit_entry;
    return Status::OK();
  }

  // If this commit does not correspond to the first replicate message in the pending
  // replicates set we keep it to apply later...
  if ((*state->pending_replicates.begin()).first != committed_op_id.index()) {
    if (!ContainsKey(state->pending_replicates, committed_op_id.index())) {
      return Status::Corruption(Substitute("Could not find replicate for commit: $0",
                                           commit_entry->ShortDebugString()));
    }
    VLOG_WITH_PREFIX(2) << "Adding pending commit for " << committed_op_id;
    InsertOrDie(&state->pending_commits, committed_op_id.index(), commit_entry);
    return Status::OK();
  }

  // ... if it does, we apply it and all the commits that immediately follow in the sequence.
  OpId last_applied = commit_entry->commit().commited_op_id();
  RETURN_NOT_OK(ApplyCommitMessage(state, commit_entry));
  delete commit_entry;

  OpIndexToEntryMap::iterator iter = state->pending_commits.begin();
  while (iter != state->pending_commits.end()) {
    if ((*iter).first == last_applied.index() + 1) {
      gscoped_ptr<LogEntryPB> buffered_commit_entry((*iter).second);
      state->pending_commits.erase(iter++);
      last_applied = buffered_commit_entry->commit().commited_op_id();
      RETURN_NOT_OK(ApplyCommitMessage(state, buffered_commit_entry.get()));
      continue;
    }
    break;
  }

  return Status::OK();
}

bool TabletBootstrap::AreAllStoresAlreadyFlushed(const CommitMsg& commit) {
  BOOST_FOREACH(const OperationResultPB& op_result, commit.result().ops()) {
    BOOST_FOREACH(const MemStoreTargetPB& mutated_store, op_result.mutated_stores()) {
      if (!flushed_stores_.WasStoreAlreadyFlushed(mutated_store)) {
        return false;
      }
    }
  }
  return true;
}

bool TabletBootstrap::AreAnyStoresAlreadyFlushed(const CommitMsg& commit) {
  BOOST_FOREACH(const OperationResultPB& op_result, commit.result().ops()) {
    BOOST_FOREACH(const MemStoreTargetPB& mutated_store, op_result.mutated_stores()) {
      if (flushed_stores_.WasStoreAlreadyFlushed(mutated_store)) {
        return true;
      }
    }
  }
  return false;
}

Status TabletBootstrap::CheckOrphanedCommitAlreadyFlushed(const CommitMsg& commit) {
  if (!AreAllStoresAlreadyFlushed(commit)) {
    TabletSuperBlockPB super;
    WARN_NOT_OK(meta_->ToSuperBlock(&super), LogPrefix() + "Couldn't build TabletSuperBlockPB");
    return Status::Corruption(Substitute("CommitMsg was orphaned but it referred to "
        "unflushed stores. Commit: $0. TabletMetadata: $1", commit.ShortDebugString(),
        super.ShortDebugString()));
  }
  return Status::OK();
}

Status TabletBootstrap::ApplyCommitMessage(ReplayState* state, LogEntryPB* commit_entry) {

  const OpId& committed_op_id = commit_entry->commit().commited_op_id();
  VLOG_WITH_PREFIX(2) << "Applying commit for " << committed_op_id;
  gscoped_ptr<LogEntryPB> pending_replicate_entry;

  // They should also have an associated replicate index (it may have been in a
  // deleted log segment though).
  pending_replicate_entry.reset(EraseKeyReturnValuePtr(&state->pending_replicates,
                                                       committed_op_id.index()));

  if (pending_replicate_entry != NULL) {
    // We found a replicate with the same index, make sure it also has the same
    // term.
    if (!OpIdEquals(committed_op_id, pending_replicate_entry->replicate().id())) {
      string error_msg = Substitute("Committed operation's OpId: $0 didn't match the"
          "commit message's committed OpId: $1. Pending operation: $2, Commit message: $3",
          pending_replicate_entry->replicate().id().ShortDebugString(),
          committed_op_id.ShortDebugString(),
          pending_replicate_entry->replicate().ShortDebugString(),
          commit_entry->commit().ShortDebugString());
      LOG_WITH_PREFIX(DFATAL) << error_msg;
      return Status::Corruption(error_msg);
    }
    RETURN_NOT_OK(HandleEntryPair(pending_replicate_entry.get(), commit_entry));
    stats_.ops_committed++;
  } else {
    stats_.orphaned_commits++;
    RETURN_NOT_OK(CheckOrphanedCommitAlreadyFlushed(commit_entry->commit()));
  }

  return Status::OK();
}

// Never deletes 'replicate_entry' or 'commit_entry'.
Status TabletBootstrap::HandleEntryPair(LogEntryPB* replicate_entry, LogEntryPB* commit_entry) {
  const char* error_fmt = "Failed to play $0 request. ReplicateMsg: { $1 }, CommitMsg: { $2 }";

#define RETURN_NOT_OK_REPLAY(ReplayMethodName, replicate, commit) \
  RETURN_NOT_OK_PREPEND(ReplayMethodName(replicate, commit), \
                        Substitute(error_fmt, OperationType_Name(op_type), \
                                   replicate->ShortDebugString(), commit.ShortDebugString()))

  ReplicateMsg* replicate = replicate_entry->mutable_replicate();
  const CommitMsg& commit = commit_entry->commit();
  OperationType op_type = commit.op_type();

  switch (op_type) {
    case WRITE_OP:
      RETURN_NOT_OK_REPLAY(PlayWriteRequest, replicate, commit);
      break;

    case ALTER_SCHEMA_OP:
      RETURN_NOT_OK_REPLAY(PlayAlterSchemaRequest, replicate, commit);
      break;

    case CHANGE_CONFIG_OP:
      RETURN_NOT_OK_REPLAY(PlayChangeConfigRequest, replicate, commit);
      break;

    case NO_OP:
      RETURN_NOT_OK_REPLAY(PlayNoOpRequest, replicate, commit);
      break;

    default:
      return Status::IllegalState(Substitute("Unsupported commit entry type: $0",
                                             commit.op_type()));
  }

#undef RETURN_NOT_OK_REPLAY

  // Non-tablet operations should not advance the safe time, because they are
  // not started serially and so may have timestamps that are out of order.
  if (op_type == NO_OP || op_type == CHANGE_CONFIG_OP) {
    return Status::OK();
  }

  // Handle safe time advancement:
  //
  // If this operation has an external consistency mode other than COMMIT_WAIT, we know that no
  // future transaction will have a timestamp that is lower than it, so we can just advance the
  // safe timestamp to this operation's timestamp.
  //
  // If the hybrid clock is disabled, all transactions will fall into this category.
  Timestamp safe_time;
  if (replicate->write_request().external_consistency_mode() != COMMIT_WAIT) {
    safe_time = Timestamp(replicate->timestamp());
  // ... else we set the safe timestamp to be the transaction's timestamp minus the maximum clock
  // error. This opens the door for problems if the flags changed across reboots, but this is
  // unlikely and the problem would manifest itself immediately and clearly (mvcc would complain
  // the operation is already committed, with a CHECK failure).
  } else {
    DCHECK(clock_->SupportsExternalConsistencyMode(COMMIT_WAIT)) << "The provided clock does not"
        "support COMMIT_WAIT external consistency mode.";
    safe_time = server::HybridClock::AddPhysicalTimeToTimestamp(
        Timestamp(replicate->timestamp()),
        MonoDelta::FromMicroseconds(-FLAGS_max_clock_sync_error_usec));
  }
  tablet_->mvcc_manager()->OfflineAdjustSafeTime(safe_time);

  return Status::OK();
}

void TabletBootstrap::DumpReplayStateToLog(const ReplayState& state) {
  // Dump the replay state, this will log the pending replicates as well as the pending commits,
  // which might be useful for debugging.
  vector<string> state_dump;
  state.DumpReplayStateToStrings(&state_dump);
  BOOST_FOREACH(const string& string, state_dump) {
    LOG_WITH_PREFIX(INFO) << string;
  }
}

Status TabletBootstrap::PlaySegments(ConsensusBootstrapInfo* consensus_info) {
  ReplayState state;
  log::SegmentSequence segments;
  RETURN_NOT_OK(log_reader_->GetSegmentsSnapshot(&segments));

  // The first thing to do is to rewind the tablet's schema back to the schema
  // as of the point in time where the logs begin. We must replay the writes
  // in the logs with the correct point-in-time schema.
  if (!segments.empty()) {
    const scoped_refptr<ReadableLogSegment>& segment = segments[0];
    // Set the point-in-time schema for the tablet based on the log header.
    Schema pit_schema;
    RETURN_NOT_OK_PREPEND(SchemaFromPB(segment->header().schema(), &pit_schema),
                          "Couldn't decode log segment schema");
    RETURN_NOT_OK_PREPEND(tablet_->RewindSchemaForBootstrap(
                              pit_schema, segment->header().schema_version()),
                          "couldn't set point-in-time schema");
  }

  // We defer opening the log until here, so that we properly reproduce the
  // point-in-time schema from the log we're reading into the log we're
  // writing.
  RETURN_NOT_OK_PREPEND(OpenNewLog(), "Failed to open new log");

  int segment_count = 0;
  BOOST_FOREACH(const scoped_refptr<ReadableLogSegment>& segment, segments) {
    vector<LogEntryPB*> entries;
    ElementDeleter deleter(&entries);
    // TODO: Optimize this to not read the whole thing into memory?
    Status read_status = segment->ReadEntries(&entries);
    for (int entry_idx = 0; entry_idx < entries.size(); ++entry_idx) {
      LogEntryPB* entry = entries[entry_idx];
      Status s = HandleEntry(&state, entry);
      if (!s.ok()) {
        DumpReplayStateToLog(state);
        RETURN_NOT_OK_PREPEND(s, DebugInfo(tablet_->tablet_id(),
                                           segment->header().sequence_number(),
                                           entry_idx, segment->path(),
                                           *entry));
      }

      // If HandleEntry returns OK, then it has taken ownership of the entry.
      // So, we have to remove it from the entries vector to avoid it getting
      // freed by ElementDeleter.
      entries[entry_idx] = NULL;
    }

    // If the LogReader failed to read for some reason, we'll still try to
    // replay as many entries as possible, and then fail with Corruption.
    // TODO: this is sort of scary -- why doesn't LogReader expose an
    // entry-by-entry iterator-like API instead? Seems better to avoid
    // exposing the idea of segments to callers.
    if (PREDICT_FALSE(!read_status.ok())) {
      return Status::Corruption(Substitute("Error reading Log Segment of tablet $0: $1 "
                                           "(Read up to entry $2 of segment $3, in path $4)",
                                           tablet_->tablet_id(),
                                           read_status.ToString(),
                                           entries.size(),
                                           segment->header().sequence_number(),
                                           segment->path()));
    }

    // TODO: could be more granular here and log during the segments as well,
    // plus give info about number of MB processed, but this is better than
    // nothing.
    listener_->StatusMessage(Substitute("Bootstrap replayed $0/$1 log segments. "
                                        "Stats: $2. Pending: $3 replicates",
                                        segment_count + 1, log_reader_->num_segments(),
                                        stats_.ToString(),
                                        state.pending_replicates.size()));
    segment_count++;
  }

  // If we have non-applied commits they all must belong to pending operations and
  // they should only pertain to unflushed stores.
  if (!state.pending_commits.empty()) {
    BOOST_FOREACH(const OpIndexToEntryMap::value_type& entry, state.pending_commits) {
      if (!ContainsKey(state.pending_replicates, entry.first)) {
        DumpReplayStateToLog(state);
        return Status::Corruption("Had orphaned commits at the end of replay.");
      }
      if (AreAnyStoresAlreadyFlushed(entry.second->commit())) {
        DumpReplayStateToLog(state);
        TabletSuperBlockPB super;
        WARN_NOT_OK(meta_->ToSuperBlock(&super), "Couldn't build TabletSuperBlockPB.");
        return Status::Corruption(Substitute("CommitMsg was pending but it referred to "
            "flushed stores. Commit: $0. TabletMetadata: $1",
            entry.second->commit().ShortDebugString(), super.ShortDebugString()));
      }
    }
  }

  // Note that we don't pass the information contained in the pending commits along with
  // ConsensusBootstrapInfo. We know that this is safe as they must refer to unflushed
  // stores (we make doubly sure above).
  //
  // Example/Explanation:
  // Say we have two different operations that touch the same row, one insert and one
  // mutate. Since we use Early Lock Release the commit for the second (mutate) operation
  // might end up in the log before the insert's commit. This wouldn't matter since
  // we replay in order, but a corner case here is that we might crash before we
  // write the commit for the insert, meaning it might not be present at all.
  //
  // One possible log for this situation would be:
  // - Replicate 10.10 (insert)
  // - Replicate 10.11 (mutate)
  // - Commit    10.11 (mutate)
  // ~CRASH while Commit 10.10 is in-flight~
  //
  // We can't replay 10.10 during bootstrap because we haven't seen its commit, but
  // since we can't replay out-of-order we won't replay 10.11 either, in fact we'll
  // pass them both as "pending" to consensus to be applied again.
  //
  // The reason why it is safe to simply disregard 10.11's commit is that we know that
  // it must refer only to unflushed stores. We know this because one important flush/compact
  // pre-condition is:
  // - No flush will become visible on reboot (meaning we won't durably update the tablet
  //   metadata), unless the snapshot under which the flush/compact was performed has no
  //   in-flight transactions and all the messages that are in-flight to the log are durable.
  //
  // In our example this means that if we had flushed/compacted after 10.10 was applied
  // (meaning losing the commit message would lead to corruption as we might re-apply it)
  // then the commit for 10.10 would be durable. Since it isn't then no flush/compaction
  // occurred after 10.10 was applied and thus we can disregard the commit message for
  // 10.11 and simply apply both 10.10 and 10.11 as if we hadn't applied them before.
  //
  // This generalizes to:
  // - If a committed replicate message with index Y is missing a commit message,
  //   no later committed replicate message (with index > Y) is visible across reboots
  //   in the tablet data.

  DumpReplayStateToLog(state);

  // Set up the ConsensusBootstrapInfo structure for the caller.
  BOOST_FOREACH(OpIndexToEntryMap::value_type& e, state.pending_replicates) {
    consensus_info->orphaned_replicates.push_back(e.second->release_replicate());
  }
  consensus_info->last_id = state.prev_op_id;
  consensus_info->last_committed_id = state.committed_op_id;

  return Status::OK();
}

Status TabletBootstrap::AppendCommitMsg(const CommitMsg& commit_msg) {
  LogEntryPB commit_entry;
  commit_entry.set_type(log::COMMIT);
  CommitMsg* commit = commit_entry.mutable_commit();
  commit->CopyFrom(commit_msg);
  return log_->Append(&commit_entry);
}

Status TabletBootstrap::PlayWriteRequest(ReplicateMsg* replicate_msg,
                                         const CommitMsg& commit_msg) {
  DCHECK(replicate_msg->has_timestamp());
  WriteRequestPB* write = replicate_msg->mutable_write_request();

  WriteTransactionState tx_state(NULL, write, NULL);
  tx_state.mutable_op_id()->CopyFrom(replicate_msg->id());
  tx_state.set_timestamp(Timestamp(replicate_msg->timestamp()));

  tablet_->StartTransaction(&tx_state);
  tablet_->StartApplying(&tx_state);

  // Use committed OpId for mem store anchoring.
  tx_state.mutable_op_id()->CopyFrom(replicate_msg->id());

  if (write->has_row_operations()) {
    // TODO: get rid of redundant params below - they can be gotten from the Request
    RETURN_NOT_OK(PlayRowOperations(&tx_state,
                                    write->schema(),
                                    write->row_operations(),
                                    commit_msg.result()));
  }

  // Append the commit msg to the log but replace the result with the new one.
  LogEntryPB commit_entry;
  commit_entry.set_type(log::COMMIT);
  CommitMsg* commit = commit_entry.mutable_commit();
  commit->CopyFrom(commit_msg);
  tx_state.ReleaseTxResultPB(commit->mutable_result());
  RETURN_NOT_OK(log_->Append(&commit_entry));

  return Status::OK();
}

Status TabletBootstrap::PlayAlterSchemaRequest(ReplicateMsg* replicate_msg,
                                               const CommitMsg& commit_msg) {
  AlterSchemaRequestPB* alter_schema = replicate_msg->mutable_alter_schema_request();

  // Decode schema
  Schema schema;
  RETURN_NOT_OK(SchemaFromPB(alter_schema->schema(), &schema));

  AlterSchemaTransactionState tx_state(NULL, alter_schema, NULL);

  // TODO(KUDU-860): we should somehow distinguish if an alter table failed on its original
  // attempt (e.g due to being an invalid request, or a request with a too-early
  // schema version).

  RETURN_NOT_OK(tablet_->CreatePreparedAlterSchema(&tx_state, &schema));

  // Apply the alter schema to the tablet
  RETURN_NOT_OK_PREPEND(tablet_->AlterSchema(&tx_state), "Failed to AlterSchema:");

  // Also update the log information. Normally, the AlterSchema() call above
  // takes care of this, but our new log isn't hooked up to the tablet yet.
  log_->SetSchemaForNextLogSegment(schema, tx_state.schema_version());

  return AppendCommitMsg(commit_msg);
}

Status TabletBootstrap::PlayChangeConfigRequest(ReplicateMsg* replicate_msg,
                                                const CommitMsg& commit_msg) {
  ChangeConfigRecordPB* change_config = replicate_msg->mutable_change_config_record();
  RaftConfigPB config = change_config->new_config();

  int64_t cmeta_opid_index =  cmeta_->committed_config().opid_index();
  if (replicate_msg->id().index() > cmeta_opid_index) {
    DCHECK(!config.has_opid_index());
    config.set_opid_index(replicate_msg->id().index());
    VLOG_WITH_PREFIX(1) << "WAL replay found Raft configuration with log index "
                        << config.opid_index()
                        << " that is greater than the committed config's index "
                        << cmeta_opid_index
                        << ". Applying this configuration change.";
    cmeta_->set_committed_config(config);
    // We flush once at the end of bootstrap.
  } else {
    VLOG_WITH_PREFIX(1) << "WAL replay found Raft configuration with log index "
                        << replicate_msg->id().index()
                        << ", which is less than or equal to the committed "
                        << "config's index " << cmeta_opid_index << ". "
                        << "Skipping application of this config change.";
  }

  return AppendCommitMsg(commit_msg);
}

Status TabletBootstrap::PlayNoOpRequest(ReplicateMsg* replicate_msg, const CommitMsg& commit_msg) {
  return AppendCommitMsg(commit_msg);
}

Status TabletBootstrap::PlayRowOperations(WriteTransactionState* tx_state,
                                          const SchemaPB& schema_pb,
                                          const RowOperationsPB& ops_pb,
                                          const TxResultPB& result) {
  Schema inserts_schema;
  RETURN_NOT_OK_PREPEND(SchemaFromPB(schema_pb, &inserts_schema),
                        "Couldn't decode client schema");

  arena_.Reset();

  RETURN_NOT_OK_PREPEND(tablet_->DecodeWriteOperations(&inserts_schema, tx_state),
                        Substitute("Could not decode row operations: $0",
                                   ops_pb.ShortDebugString()));
  CHECK_EQ(tx_state->row_ops().size(), result.ops_size());

  // Run AcquireRowLocks, Apply, etc!
  RETURN_NOT_OK_PREPEND(tablet_->AcquireRowLocks(tx_state),
                        "Failed to acquire row locks");

  RETURN_NOT_OK(FilterAndApplyOperations(tx_state, result));

  return Status::OK();
}

Status TabletBootstrap::FilterAndApplyOperations(WriteTransactionState* tx_state,
                                                 const TxResultPB& orig_result) {
  int32_t op_idx = 0;
  BOOST_FOREACH(RowOp* op, tx_state->row_ops()) {
    const OperationResultPB& orig_op_result = orig_result.ops(op_idx++);

    // check if the operation failed in the original transaction
    if (PREDICT_FALSE(orig_op_result.has_failed_status())) {
      Status status = StatusFromPB(orig_op_result.failed_status());
      if (VLOG_IS_ON(1)) {
        VLOG_WITH_PREFIX(1) << "Skipping operation that originally resulted in error. OpId: "
                            << tx_state->op_id().DebugString() << " op index: "
                            << op_idx - 1 << " original error: "
                            << status.ToString();
      }
      op->SetFailed(status);
      continue;
    }

    // Check if it should be filtered out because it's already flushed.
    switch (op->decoded_op.type) {
      case RowOperationsPB::INSERT:
        stats_.inserts_seen++;
        if (!orig_op_result.flushed()) {
          RETURN_NOT_OK(FilterInsert(tx_state, op, orig_op_result));
        } else {
          op->SetAlreadyFlushed();
          stats_.inserts_ignored++;
          continue;
        }
        break;
      case RowOperationsPB::UPDATE:
      case RowOperationsPB::DELETE:
        stats_.mutations_seen++;
        if (!orig_op_result.flushed()) {
          RETURN_NOT_OK(FilterMutate(tx_state, op, orig_op_result));
        } else {
          op->SetAlreadyFlushed();
          stats_.mutations_ignored++;
          continue;
        }
        break;
      default:
        LOG_WITH_PREFIX(FATAL) << "Bad op type: " << op->decoded_op.type;
        break;
    }
    if (op->result != NULL) {
      continue;
    }

    // Actually apply it.
    tablet_->ApplyRowOperation(tx_state, op);
    DCHECK(op->result != NULL);

    // We expect that the above Apply() will always succeed, because we're
    // applying an operation that we know succeeded before the server
    // restarted. If it doesn't succeed, something is wrong and we are
    // diverging from our prior state, so bail.
    if (op->result->has_failed_status()) {
      return Status::Corruption("Operation which previously succeeded failed "
                                "during log replay",
                                Substitute("Op: $0\nFailure: $1",
                                           op->ToString(*tablet_->schema()),
                                           op->result->failed_status().ShortDebugString()));
    }
  }
  return Status::OK();
}

Status TabletBootstrap::FilterInsert(WriteTransactionState* tx_state,
                                     RowOp* op,
                                     const OperationResultPB& op_result) {
  DCHECK_EQ(op->decoded_op.type, RowOperationsPB::INSERT);

  if (PREDICT_FALSE(op_result.mutated_stores_size() != 1 ||
                    !op_result.mutated_stores(0).has_mrs_id())) {
    return Status::Corruption(Substitute("Insert operation result must have an mrs_id: $0",
                                         op_result.ShortDebugString()));
  }
  // check if the insert is already flushed
  if (flushed_stores_.WasStoreAlreadyFlushed(op_result.mutated_stores(0))) {
    if (VLOG_IS_ON(1)) {
      VLOG_WITH_PREFIX(1) << "Skipping insert that was already flushed. OpId: "
                          << tx_state->op_id().DebugString()
                          << " flushed to: " << op_result.mutated_stores(0).mrs_id()
                          << " latest durable mrs id: "
                          << tablet_->metadata()->last_durable_mrs_id();
    }

    op->SetAlreadyFlushed();
    stats_.inserts_ignored++;
  }
  return Status::OK();
}

Status TabletBootstrap::FilterMutate(WriteTransactionState* tx_state,
                                     RowOp* op,
                                     const OperationResultPB& op_result) {
  DCHECK(op->decoded_op.type == RowOperationsPB::UPDATE ||
         op->decoded_op.type == RowOperationsPB::DELETE)
    << RowOperationsPB::Type_Name(op->decoded_op.type);

  int num_mutated_stores = op_result.mutated_stores_size();
  if (PREDICT_FALSE(num_mutated_stores == 0 || num_mutated_stores > 2)) {
    return Status::Corruption(Substitute("Mutations must have one or two mutated_stores: $0",
                                         op_result.ShortDebugString()));
  }

  // The mutation may have been duplicated, so we'll check whether any of the
  // output targets was "unflushed".
  int num_unflushed_stores = 0;
  BOOST_FOREACH(const MemStoreTargetPB& mutated_store, op_result.mutated_stores()) {
    if (!flushed_stores_.WasStoreAlreadyFlushed(mutated_store)) {
      num_unflushed_stores++;
    } else {
      if (VLOG_IS_ON(1)) {
        string mutation = op->decoded_op.changelist.ToString(*tablet_->schema());
        VLOG_WITH_PREFIX(1) << "Skipping mutation to " << mutated_store.ShortDebugString()
                            << " that was already flushed. "
                            << "OpId: " << tx_state->op_id().DebugString();
      }
    }
  }

  if (num_unflushed_stores == 0) {
    // The mutation was fully flushed.
    op->SetFailed(Status::AlreadyPresent("Update was already flushed."));
    stats_.mutations_ignored++;
    return Status::OK();
  }

  if (num_unflushed_stores == 2) {
    // 18:47 < dralves> off the top of my head, if we crashed before writing the meta
    //                  at the end of a flush/compation then both mutations could
    //                  potentually be considered unflushed
    // This case is not currently covered by any tests -- we need to add test coverage
    // for this. See KUDU-218. It's likely the correct behavior is just to apply the edit,
    // ie not fatal below.
    LOG_WITH_PREFIX(DFATAL) << "TODO: add test coverage for case where op is unflushed "
                            << "in both duplicated targets";
  }

  return Status::OK();
}

Status TabletBootstrap::UpdateClock(uint64_t timestamp) {
  Timestamp ts;
  RETURN_NOT_OK(ts.FromUint64(timestamp));
  RETURN_NOT_OK(clock_->Update(ts));
  return Status::OK();
}

string TabletBootstrap::LogPrefix() const {
  return Substitute("T $0 P $1: ", meta_->tablet_id(), meta_->fs_manager()->uuid());
}

Status FlushedStoresSnapshot::InitFrom(const TabletMetadata& meta) {
  CHECK(flushed_dms_by_drs_id_.empty()) << "already initted";
  last_durable_mrs_id_ = meta.last_durable_mrs_id();
  BOOST_FOREACH(const shared_ptr<RowSetMetadata>& rsmd, meta.rowsets()) {
    if (!InsertIfNotPresent(&flushed_dms_by_drs_id_, rsmd->id(),
                            rsmd->last_durable_redo_dms_id())) {
      return Status::Corruption(Substitute(
          "Duplicate DRS ID $0 in tablet metadata. "
          "Found DRS $0 with last durable redo DMS ID $1 while trying to "
          "initialize DRS $0 with last durable redo DMS ID $2",
          rsmd->id(),
          flushed_dms_by_drs_id_[rsmd->id()],
          rsmd->last_durable_redo_dms_id()));
    }
  }
  return Status::OK();
}

bool FlushedStoresSnapshot::WasStoreAlreadyFlushed(const MemStoreTargetPB& target) const {
  if (target.has_mrs_id()) {
    DCHECK(!target.has_rs_id());
    DCHECK(!target.has_dms_id());

    // The original mutation went to the MRS. It is flushed if it went to an MRS
    // with a lower ID than the latest flushed one.
    return target.mrs_id() <= last_durable_mrs_id_;
  } else {
    // The original mutation went to a DRS's delta store.
    int64_t last_durable_dms_id;
    if (!FindCopy(flushed_dms_by_drs_id_, target.rs_id(), &last_durable_dms_id)) {
      // if we have no data about this RowSet, then it must have been flushed and
      // then deleted.
      // TODO: how do we avoid a race where we get an update on a rowset before
      // it is persisted? add docs about the ordering of flush.
      return true;
    }

    // If the original rowset that we applied the edit to exists, check whether
    // the edit was in a flushed DMS or a live one.
    if (target.dms_id() <= last_durable_dms_id) {
      return true;
    }

    return false;
  }
}

} // namespace tablet
} // namespace kudu

ext: cc

max_stars_repo_name: kv-zuiwanyuan/kudu

max_issues_repo_name: kv-zuiwanyuan/kudu

max_forks_repo_name: kv-zuiwanyuan/kudu


text:
/***************************************************************************
 * Copyright 1998-2018 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxCoreRender.                                   *
 *                                                                         *
 * Licensed under the Apache License, Version 2.0 (the "License");         *
 * you may not use this file except in compliance with the License.        *
 * You may obtain a copy of the License at                                 *
 *                                                                         *
 *     http://www.apache.org/licenses/LICENSE-2.0                          *
 *                                                                         *
 * Unless required by applicable law or agreed to in writing, software     *
 * distributed under the License is distributed on an "AS IS" BASIS,       *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.*
 * See the License for the specific language governing permissions and     *
 * limitations under the License.                                          *
 ***************************************************************************/

#include <memory>

#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>

#include "luxrays/utils/serializationutils.h"
#include "slg/scene/scene.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// Scene serialization
//------------------------------------------------------------------------------

BOOST_CLASS_EXPORT_IMPLEMENT(slg::Scene)

Scene *Scene::LoadSerialized(const std::string &fileName) {
	SerializationInputFile sif(fileName);

	Scene *scene;
	sif.GetArchive() >> scene;

	if (!sif.IsGood())
		throw runtime_error("Error while loading serialized scene: " + fileName);

	return scene;
}

void Scene::SaveSerialized(const std::string &fileName, const Scene *scene) {
	SerializationOutputFile sof(fileName);

	sof.GetArchive() << scene;

	if (!sof.IsGood())
		throw runtime_error("Error while saving serialized scene: " + fileName);

	sof.Flush();

	SLG_LOG("Scene saved: " << (sof.GetPosition() / 1024) << " Kbytes");
}

max_stars_repo_name: aedancullen/LuxCore

max_issues_repo_name: aedancullen/LuxCore

max_forks_repo_name: aedancullen/LuxCore


text:
////////////////////////////////////////////////////////////////////////////////
// CorotatedLinearElasticity.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  A general linear elastic material model with geometric nonlinearities for
//  small strain, large deformation applications.
//
//  We use a (right) polar decomposition F = RS to remove the rigid rotation
//  component R from the deformation gradient F. The resulting large deformation
//  strain is S - I, also known as the Biot strain.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  05/19/2020 12:15:32
////////////////////////////////////////////////////////////////////////////////
#ifndef COROTATEDLINEARELASTICITY_HH
#define COROTATEDLINEARELASTICITY_HH

#include <Eigen/Dense>
#include "EnergyTraits.hh"
#include "Tensor.hh"

// Dimension-specific calculations
template<typename _Real, size_t N>
struct CRQuantities;

template<typename _Real>
struct CRQuantities<_Real, 3> {
    using GType    = Eigen::Matrix<_Real, 3, 3>;
    using IRotType = Eigen::Matrix<_Real, 3, 1>; // infinitesimal rotation representation
    using Mat      = Eigen::Matrix<_Real, 3, 3>;

    template<typename Derived>
    static GType getG(const Eigen::MatrixBase<Derived> &S) { return S.trace() * GType::Identity() - S; }

    template<typename Derived>
    static GType getGinv(const Eigen::MatrixBase<Derived> &G) { return G.inverse(); }

    // Extract a vector representing the skew symmetric part 0.5 (A - A^T)
    //     0 -c  b      [a]
    //     c  0 -a  ==> [b]
    //    -b  a  0      [c]
    // (This is the vector `w` whose cross product `w x v` equals `0.5 (A - A^T) v`.)
    template<typename Derived>
    static IRotType sk_inv(const Eigen::MatrixBase<Derived> &A) {
        return IRotType(0.5 * (A(2, 1) - A(1, 2)),
                        0.5 * (A(0, 2) - A(2, 0)),
                        0.5 * (A(1, 0) - A(0, 1)));
    }

    // B * sk(w)
    template<typename Derived>
    static Mat right_mul_sk(const Eigen::MatrixBase<Derived> &B, const IRotType &w) {
        return B.rowwise().cross(w);
    }
};

template<typename _Real>
struct CRQuantities<_Real, 2> {
    using GType    = _Real;
    using IRotType = _Real;
    using Mat      = Eigen::Matrix<_Real, 2, 2>;

    template<typename Derived>
    static GType getG(const Eigen::MatrixBase<Derived> &S) { return S.trace(); }

    static GType getGinv(_Real G) { return 1.0 / G; }

    // Extract a scalar representing the skew symmetric part 0.5 (A - A^T)
    //   0 -a
    //   a  0
    // (This scalar represents the counterclockwise infinitesimal rotation
    //  applied by `0.5 (A - A^T)`
    template<typename Derived>
    static IRotType sk_inv(const Eigen::MatrixBase<Derived> &A) {
        return 0.5 * (A(1, 0) - A(0, 1));
    }

    // B * sk(w)
    template<typename Derived>
    static Mat right_mul_sk(const Eigen::MatrixBase<Derived> &B, const IRotType &w) {
        Mat result;
        result << w * B.col(1),
                 -w * B.col(0);
        return result;
    }
};

template <typename _Real, size_t _Dimension>
struct CorotatedLinearElasticity : public Concepts::CRLinearElaticEnergy {
    using SMatrix = SymmetricMatrixValue<_Real, _Dimension>;
    static constexpr EDensityType EDType = EDensityType::FBased;

    static constexpr size_t Dimension = _Dimension;
    static constexpr size_t N         = Dimension;
    using Real = _Real;
    using Matrix  = Eigen::Matrix<_Real, _Dimension, _Dimension>;
    using ETensor = ElasticityTensor<_Real, _Dimension>;
    using CRQ     = CRQuantities<_Real, _Dimension>;

    // We can use simplified formulas if we know `elasticity_tensor` is isotropic. This is
    // specified with the `isotropic` argument.
    CorotatedLinearElasticity(const ETensor& elasticity_tensor, bool isotropic = false) :
        m_elasticity_tensor(elasticity_tensor), m_isotropic(isotropic) {
        setDeformationGradient(Matrix::Identity());
    }

    // Constructor copying material properties only, not the current deformation
    CorotatedLinearElasticity(const CorotatedLinearElasticity &other, UninitializedDeformationTag &&)
        : m_elasticity_tensor(other.m_elasticity_tensor), m_isotropic(other.m_isotropic) { }

    void setDeformationGradient(const Matrix &F, const EvalLevel elevel = EvalLevel::Full) {
        m_F = F;
        Eigen::JacobiSVD<Matrix> svd;
        svd.compute(F, Eigen::ComputeFullU | Eigen::ComputeFullV );
        m_R = svd.matrixU() * svd.matrixV().transpose();
        if (m_R.determinant() < 0) {
            Matrix W = svd.matrixV();
            W.col(svd.matrixV().cols() - 1) *= -1;
            m_R = svd.matrixU() * W.transpose();
        }
        m_S = m_R.transpose() * F;

        // Analog to infinitesimal strain for linear elasticity.
        m_biotStrain = m_S - Matrix::Identity();

        // Analog to Cauchy stress for linear elasticity.
        m_biotStress = m_elasticity_tensor.doubleContract(SMatrix(m_biotStrain)).toMatrix();

        if (elevel == EvalLevel::EnergyOnly) return;

        m_G    = CRQ::getG(m_S);
        m_Ginv = CRQ::getGinv(m_G);
        if (!m_isotropic) {
            // 2 sk(G^{-1} sk^{-1}(sigma * S)) := sk(g)
            m_g = 2 * m_Ginv * CRQ::sk_inv(m_biotStress * m_S);
            m_pk1_stress = m_R * m_biotStress - CRQ::right_mul_sk(m_R, m_g);
        }
        else {
            // If the elasticity tensor is isotropic, then sigma and S commute
            // (Biot stress and stretch factor share eigenvectors) and all
            // "rotational stress" terms involving sk(g) vanish.
            m_pk1_stress = m_R * m_biotStress;
        }
    }

    const Matrix &getDeformationGradient() const { return m_F; }

    _Real energy() const {
        return 0.5 * doubleContract(m_biotStress, m_biotStrain);
    }

    // PK1 stress
    _Real denergy(const Matrix& dF) const { return doubleContract(denergy(), dF); }

    // Asymmetric!
    Matrix denergy() const {
        return m_pk1_stress;
    }

    // Symmetric!
    Matrix PK2Stress() const { return m_F.inverse() * denergy(); }

    const Matrix &R() const { return m_R; }
    const Matrix &S() const { return m_S; }
    const Matrix &biotStress() const { return m_biotStress; }

    template<class Mat_>
    Matrix delta_R(const Mat_ &dF) const {
        typename CRQ::IRotType w = 2 * m_Ginv * CRQ::sk_inv(m_R.transpose() * dF);
        return CRQ::right_mul_sk(m_R, w);
    }

    template<class Mat_>
    Matrix delta_S(const Mat_ &dF, const Matrix &dR) const {
        return dR.transpose() * m_F + m_R.transpose() * dF;
    }

    Matrix delta_sigma(const Matrix &dS) const {
        return m_elasticity_tensor.doubleContract(SMatrix(dS)).toMatrix();
    }

    template<class Mat_>
    Matrix delta_denergy(const Mat_ &dF) const {
        Matrix dR     = delta_R(dF);
        Matrix dS     = delta_S(dF, dR);
        Matrix dsigma = delta_sigma(dS);

        Matrix result = dR * m_biotStress + m_R * dsigma;

        if (!m_isotropic) {
            typename CRQ::IRotType dg = m_Ginv * (2 * CRQ::sk_inv(dsigma * m_S + m_biotStress * dS) - CRQ::getG(dS) * m_g);
            result -= CRQ::right_mul_sk(dR, m_g) + CRQ::right_mul_sk(m_R, dg);
        }
        return result;
    }

    _Real d2energy(const Matrix &dF_lhs, const Matrix &dF_rhs) const {
        return doubleContract(delta_denergy(dF_lhs), dF_rhs);
    }

    template<class Mat_, class Mat2_>
    Matrix delta2_denergy(const Mat_ &dF_a, const Mat2_ &dF_b) const {
        typename CRQ::IRotType w_a = 2 * m_Ginv * CRQ::sk_inv(m_R.transpose() * dF_a);
        Matrix dR_a = CRQ::right_mul_sk(m_R, w_a),
               dR_b = delta_R(dF_b);
        Matrix dS_a = delta_S(dF_a, dR_a),
               dS_b = delta_S(dF_b, dR_b);
        Matrix dsigma_a = delta_sigma(dS_a);
        Matrix dsigma_b = delta_sigma(dS_b);

        Matrix d2R, d2S, d2sigma;
        d2R = CRQ::right_mul_sk(dR_b, w_a)
            + CRQ::right_mul_sk(m_R, m_Ginv * (2 * CRQ::sk_inv(dR_b.transpose() * dF_a) - CRQ::getG(dS_b) * w_a));

        d2S = d2R.transpose() * m_F + dR_a.transpose() * dF_b + dR_b.transpose() * dF_a;
        d2sigma = delta_sigma(d2S);

        Matrix result = d2R * m_biotStress + dR_a * dsigma_b + dR_b * dsigma_a + m_R * d2sigma;

        if (!m_isotropic) {
            typename CRQ::IRotType dg_a = m_Ginv * (2 * CRQ::sk_inv(dsigma_a * m_S + m_biotStress * dS_a) - CRQ::getG(dS_a) * m_g),
                                   dg_b = m_Ginv * (2 * CRQ::sk_inv(dsigma_b * m_S + m_biotStress * dS_b) - CRQ::getG(dS_b) * m_g),
                                   d2g  = m_Ginv * (2 * CRQ::sk_inv(d2sigma  * m_S + dsigma_a * dS_b + dsigma_b * dS_a + m_biotStress * d2S) - CRQ::getG(d2S) * m_g - CRQ::getG(dS_a) * dg_b - CRQ::getG(dS_b) * dg_a);

            result -= CRQ::right_mul_sk(d2R, m_g) + CRQ::right_mul_sk(dR_a, dg_b) + CRQ::right_mul_sk(dR_b, dg_a)
                    + CRQ::right_mul_sk(m_R, d2g);
        }
        return result;
    }

    bool isIsotropic() const { return m_isotropic; }

private:
    ETensor m_elasticity_tensor;
    Matrix m_F,
           m_R, m_S, // Polar decomposition
           m_biotStrain, m_biotStress, m_pk1_stress,
           m_anisotropicRotationalStress; // Skew symmetric contribution to the PK1 stress; only nonzero for anisotropic material model.
    typename CRQ::GType m_G, m_Ginv;
    typename CRQ::IRotType m_g;
    bool m_isotropic;
};

#endif /* end of include guard: COROTATEDLINEARELASTICITY_HH */

ext: hh

max_stars_repo_name: MeshFEM/MeshFEM

max_issues_repo_name: MeshFEM/MeshFEM

max_forks_repo_name: MeshFEM/MeshFEM


text:
/*
 * @Description: ceres residual block for LIO IMU pre-integration measurement
 * @Author: Ge Yao
 * @Date: 2020-11-29 15:47:49
 */
#ifndef LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_IMU_PRE_INTEGRATION_HPP_
#define LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_IMU_PRE_INTEGRATION_HPP_

#include <ceres/ceres.h>

#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <sophus/so3.hpp>

#include "glog/logging.h"

namespace sliding_window {

class FactorPRVAGIMUPreIntegration : public ceres::SizedCostFunction<15, 15, 15> {
public:
	static const int INDEX_P = 0;
	static const int INDEX_R = 3;
	static const int INDEX_V = 6;
	static const int INDEX_A = 9;
	static const int INDEX_G = 12;

  FactorPRVAGIMUPreIntegration(void) {};

	void SetT(const double &T) {
		T_ = T;
	}

	void SetGravitiy(const Eigen::Vector3d &g) {
		g_ = g;
	}

  void SetMeasurement(const Eigen::VectorXd &m) {
		m_ = m;
	}

  void SetInformation(const Eigen::MatrixXd &I) {
    I_ = I;
  }

	void SetJacobian(const Eigen::MatrixXd &J) {
		J_ = J;
	}

  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    //
    // parse parameters:
    //
    // a. pose i
    Eigen::Map<const Eigen::Vector3d>     pos_i(&parameters[0][INDEX_P]);
    Eigen::Map<const Eigen::Vector3d> log_ori_i(&parameters[0][INDEX_R]);
    const Sophus::SO3d                    ori_i = Sophus::SO3d::exp(log_ori_i);
		Eigen::Map<const Eigen::Vector3d>     vel_i(&parameters[0][INDEX_V]);
		Eigen::Map<const Eigen::Vector3d>     b_a_i(&parameters[0][INDEX_A]);
		Eigen::Map<const Eigen::Vector3d>     b_g_i(&parameters[0][INDEX_G]);

    // b. pose j
    Eigen::Map<const Eigen::Vector3d>     pos_j(&parameters[1][INDEX_P]);
    Eigen::Map<const Eigen::Vector3d> log_ori_j(&parameters[1][INDEX_R]);
    const Sophus::SO3d                    ori_j = Sophus::SO3d::exp(log_ori_j);
		Eigen::Map<const Eigen::Vector3d>     vel_j(&parameters[1][INDEX_V]);
		Eigen::Map<const Eigen::Vector3d>     b_a_j(&parameters[1][INDEX_A]);
		Eigen::Map<const Eigen::Vector3d>     b_g_j(&parameters[1][INDEX_G]);

    //
    // parse measurement:
    //
		const Eigen::Vector3d &alpha_ij = m_.block<3, 1>(INDEX_P, 0);
		const Eigen::Vector3d &theta_ij = m_.block<3, 1>(INDEX_R, 0);
		const Eigen::Vector3d  &beta_ij = m_.block<3, 1>(INDEX_V, 0);

    //
    // TODO: get square root of information matrix:
    //

    //
    // TODO: compute residual:
    //

    //
    // TODO: compute jacobians:
    //
    if ( jacobians ) {
      // compute shared intermediate results:

      if ( jacobians[0] ) {
        // a. residual, position:

        // b. residual, orientation:

        // c. residual, velocity:

        // d. residual, bias accel:

        // d. residual, bias accel:
      }

      if ( jacobians[1] ) {
        // a. residual, position:

        // b. residual, orientation:

        // c. residual, velocity:

        // d. residual, bias accel:

        // d. residual, bias accel:
      }
    }

    //
    // TODO: correct residual by square root of information matrix:
    //

    return true;
  }

private:
  static Eigen::Matrix3d JacobianRInv(const Eigen::Vector3d &w) {
      Eigen::Matrix3d J_r_inv = Eigen::Matrix3d::Identity();

      double theta = w.norm();

      if ( theta > 1e-5 ) {
          Eigen::Vector3d k = w.normalized();
          Eigen::Matrix3d K = Sophus::SO3d::hat(k);

          J_r_inv = J_r_inv
                    + 0.5 * K
                    + (1.0 - (1.0 + std::cos(theta)) * theta / (2.0 * std::sin(theta))) * K * K;
      }

      return J_r_inv;
  }

	double T_ = 0.0;

	Eigen::Vector3d g_ = Eigen::Vector3d::Zero();

  Eigen::VectorXd m_;
  Eigen::MatrixXd I_;

	Eigen::MatrixXd J_;
};

} // namespace sliding_window

#endif // LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_IMU_PRE_INTEGRATION_HPP_